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ABSTRACT 


Given  a  non-holonomic  disc  robot  V ,  its  motion  constraints  in  terms  of  maxi¬ 
mum  curvature  (/cmai)  and  rate  of  change  of  curvature  (Kmar),  a  set  W  of  rectilinear 
polygonal  obstacles  which  assemble  an  office-like  environment,  and  two  configura¬ 
tions  S  and  G  in  free(W),  this  thesis  investigates  the  planning  of  a  smooth  free 
path  which  satisfies  the  following  condition:  T>  is  allowed  backing  up  motions  at  the 
end  portions  of  the  path,  but  the  middle  portion  is  to  be  of  class  C2  in  its  entirety. 
Although  the  motion  planning  problem  of  D  amidst  polygonal  obstacles  has  been 
extensively  studied,  the  paths  considered  are  mostly  class  C 1  and  piecewise  C 2  only, 
and  are  subject  only  to  the  Kmax  constraint.  Typically,  such  paths  consist  of  straight 
line  segments  and  circular  ars  which  have  curvature  discontinuity  at  the  junction 
points.  In  order  for  V  to  follow  such  paths  physically,  V  has  to  stop  abruptly  at 
each  junction  point  to  change  curvature.  The  C 2  path  investigated  in  this  thesis 
allows  non-stopping  motion  of  V.  It  is  also  subject  to  a  further  Kmar  constraint 
to  avoid  turns  that  exceed  the  rate  of  change  of  curvature  constraint.  A  class  of 
smooth  curves  called  cubic  spirals  are  adopted  for  planning  C 2  paths.  Properties 
of  the  cubic  spiral  are  examined  in  detail.  A  framework  of  layered  motion  planning 
approach  is  proposed  to  divide  and  conquer  the  motion  planning  problem.  A  novel 
sensor-oriented  method  is  presented.  It  plans  a  spine  net  which  facilitates  T>  carry 
out  deviation  correction  using  sonar  sensors  while  following  a  motion  path. 
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I.  INTRODUCTION 


A.  BACKGROUND 


In  the  field  of  Robotics,  the  ultimate  goal  is  to  design  autonomous  robots  that 
are  artificially  intelligent.  This  thesis  is  an  investigation  of  a  humble  aspect  of  this 
goal:  the  problem  of  motion  planning  which  allows  an  autonomous  robot  plan  its  own 
motion  in  a  known  and  static  two-dimensional  environment.  Although  the  research 
in  robot  motion  planning  can  be  traced  back  to  the  late  60’s,  most  of  the  theoretical 
breakthroughs  and  practical  understandings  of  the  issue  have  been  achieved  only  in 
the  last  decade,  and  much  of  the  problem  is  still  outstanding. 

The  difficulty  of  motion  planning  can  best  be  summarized  by  J.  C.  Latombe 
[Ref.  1]  as  follows: 

At  first  glance  motion  planning  looks  relatively  simple,  since  humans  deal 
with  it  with  no  apparent  difficulty  in  their  everyday  lives.  In  fact,  as  is  also  the 
case  with  perception,  the  elementary  operative  intelligence  that  people  use  uncon¬ 
sciously  to  interact  with  their  environment . . .  turns  out  to  be  extremely  difficult  to 
duplicate  using  a  computer-controlled  robot.  It  is  true  that  some  naive  methods  can 
produce  apparently  impressive  results,  but  the  limitations  of  these  methods  quickly 
become  obvious.  The  unaware  reader  will  be  surprised  by  the  amount  of  nontrivial 
mathematical  and  algorithmic  techniques  that  are  necessary  to  build  a  reasonably 
general  and  reliable  motion  planner. 

The  level  of  complexity  of  the  problem  of  motion  planning  again  depends  on  how 
the  robot  is  being  modeled  and  what  physical  constraints  are  imposed  on  it.  To  date, 
even  in  the  simplest  case  of  the  basic  motion  planning  problem  ( see  Section  D.),  the 
problem  is  considered  solved  only  in  very  special  cases;  in  general,  the  basic  problem 
is  still  open. 

As  can  be  seen  in  the  sections  that  follows,  the  scope  of  the  motion  planning 
problem  in  this  thesis  is  carefully  defined  in  such  a  way  as  to  balance  the  interests 
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of  theoretical  research  and  engineering  applicability  within  the  allowable  time  frame 
of  the  study. 

B.  DEFINITIONS 

This  section  defines  a  list  of  terms  and  concepts  used  throughout  this  thesis. 
The  environment  for  the  motion  planning  problem  of  this  thesis  is  a  plane  on 
which  a  global  Cartesian  coordinate  system  is  defined.  A  world  W  is  a  set  of  n(>  1) 
simple  rectilinear  polygonal  obstacles, 

W  -  {Bi, . . . ,  Bn} 

where  no  pair  of  polygons  are  intersecting  or  touching.  The  free  space  ( free(W) ) 
is  the  complement  of  the  union  of  all  the  polygons  in  W  (free(W)  includes  the 
boundaries  of  the  polygons). 

A  path  7r  with  a  finite  length  /  in  free(W )  is  a  pair  7 r  =  (/,/),  where  /  is  a 
positive  real  number  and  /  is  a  continuous  and  standard  function, 

/  :  [0, /]  — »  free(W) 

A  path  is  thus  by  definition  free  (or  collision-free)  and  lies  completely  in  free(W). 

A  configuration  is  a  combination  of  position  and  orientation  ( x,y,6 ).  If  p  — 
( x,y,6 )  is  a  configuration,  “point  (p)”  stands  for  the  point  (x,y).  A  disc  robot  V  in 
free(W)  possesses  three  degree  of  freedom  which  are  represented  by  the  configura¬ 
tion  of  its  center. 

The  geometry  of  the  disc  robot  V  is  a  circle  C  =  (x,y,r0),  where  x,y  are  the 
coordinates  of  the  center  of  V  and  r0(>  0)  is  its  radius.  The  center  of  V  is  taken  as 
the  tracking  reference  point  of  a  given  reference  path.  Only  configuration  tracking 
is  considered  in  this  paper,  in  which  the  center  (reference  point)  of  V  stays  on  the 
path  and  the  orientation  of  the  center  equals  the  tangent  orientation  of  the  path. 
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Motion  of  V  is  subject  to  two  kinematic  constraints: 


•  A  finite  curvature  limitation  of  motion  represented  by  the  maximum  curvature 
(«mar)  that  the  vehicle  can  take. 

•  A  finite  rate  of  change  of  curvature  limitation  of  smooth  motion  represented 
by  the  maximum  rate  of  change  of  curvature  (^mar)-1 

The  disc  robot  T>  is  able  to  perform  both  forward  and  backing  up  (reversal)  motions. 
It  is  unable  to  rotate  about  its  center  because  of  the  kinematic  constraints  imposed 
on  it. 

A  path  is  of  class  Cn  if  it  is  differentiable  n  times  and  if  its  nth  derivatives  is 
continuous.  A  Cl  path  has  continuity  in  tangential  direction.  An  example  of  which 
is  a  path  consisting  of  circular  arcs  and  tangential  straight  line  segments.  A  C 2  path 
has  an  additional  continuity  in  curvature  k.  An  example  of  which  is  a  path  consisting 
of  cubic  spirals  and  tangential  straight  line  segments. 

A  C 1  path  is  Kmax— constrained  if  V  is  able  to  follow  it  under  configuration 
tracking  subject  to  nmax  constraint.  The  Kmax  constraint  is  typically  imposed  by  the 
maximum  steering  wheel  angle  of  the  robot. 

A  C 2  path  is  kmax— constrained  if  V  is  able  to  follow  it  under  configuration 
tracking  subject  to  kmax  constraint.  The  kmax  constraint  is  typically  imposed  by  the 
condition  that  the  robot  is  not  supposed  to  travel  below  a  speed  of  um,„(>  0)  when 
making  a  turn. 

A  path  is  feasible  if  it  is  piecewise  of  class  C 2  and  nmax— constrained,  and  if  every 
two  adjacent  C 2  path  pieces  share  a  common  vertical  tangent  forming  a  “cusp”  (see 
Figure  1.1(a)  for  an  example).  It  is  possible  for  V  to  follow  a  feasible  path  with  both 

'This  limitation  is  applicable  only  when  we  are  interested  in  smooth  motion  in  which  the  disc 
robot  V  is  not  supposed  to  stop  when  moving  along  a  path.  If  V  is  allowed  to  stop  before  making  a 
maneuvering  turn,  then  this  limitation  does  not  exist,  and  V  is  able  to  follow  any  /cm„-constrained 
path  so  long  as  there  is  tangential  continuity  anywhere  on  the  path. 
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forward  and  backing  up  motions. 

A  path  is  near  smooth  if  it  is  piecewise  of  class  C 2,  in  its  entirety  of  class  C1 
and  KmaI—  constrained  (see  Figure  1.1(b)  for  an  example).  It  is  possible  for  V  to 
follow  a  near  smooth  path  with  only  forward  motions,  but  the  robot  has  to  stop  at 
every  turn  where  there  is  a  discontinuity  in  curvature. 

A  path  is  smooth  if  it  is  entirely  of  class  C 2  and  is  both  Kmox— constrained  and 
iimaI — constrained  (see  Figure  1.1(c)  for  an  example).  It  is  possible  for  T>  to  follow 
a  smooth  path  with  non-stopping  forward  motions. 

The  following  relationship  holds: 


smooth  paths  C  near  smooth  paths  C  feasible  paths 


fa)  {  M  (  C) 

Figure  1.1:  Various  Types  of  Path 

C.  STATEMENT  OF  THE  PROBLEM 
1.  Motivation  and  Informal  Description 

The  problem  is  to  plan  a  motion  path  for  V  in  a  rectilinear  polygonal 
world  which  resembles  an  in-door  office  environment.  The  path  is  to  start  from 


an  initial  configuration  S,  end  at  a  final  configuration  G ,  and  possess  the  following 
characteristics: 

•  Since  a  typical  office  environment  is  expected  to  have  ample  leeway  for  T>  to 
navigate,  the  path  must  allow  V  to  navigate  in  a  non-stopping  and  forward- 
moving  motion. 

•  Exceptions  are  at  the  initial  and  the  final  portions  of  the  path,  where  depending 
on  the  orientations,  backing  up  motions  may  be  required  in  order  for  V  to  move 
out  of  S  or  into  G.  This  requirement  could  be  well  understood  by  the  everyday 
driving  experience  of  the  reader. 

2.  Formal  Statement 

Let  S  and  G  be  two  given  configurations  of  the  disc  robot  V  lying  com¬ 
pletely  inside  /ree(W);  S  is  called  the  initial  configuration  and  G  the  final  config¬ 
uration.  The  problem  is  to  find  an  intermediate  So  (can  be  coincident  with  S)  and 
an  intermediate  Go  (can  be  coincident  with  G),  and  plan  a  path  from  5  to  S0  to  G0 
to  G  satisfying  the  following  requirements: 

1.  The  entire  path  from  S  to  G  must  be  feasible ? 

2.  The  middle  portion  of  the  path  from  S0  to  G0  must  be  smooth. 

As  a  corollary,  the  initial  portion  of  the  path  from  S  to  So,  and  the  final  portion 
from  Go  to  G,  must  both  be  feasible. 

3.  Rationale  Behind  the  Scope  of  the  Problem 

This  subsection  explains  the  rationale  behind  the  definition  of  the  scope 
of  the  motion  planning  problem  in  this  thesis.  It  is  defined  with  the  following  con- 

2 Please  refer  to  Section  B.  for  the  definition  of  feasible  and  smooth  paths. 
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siderations  in  order  to  balance  the  interests  of  theoretical  research  and  engineering 
applicability  within  the  allowable  time  frame  of  the  study: 

1.  The  problem  should  be  manageable 

In  order  to  simplify  the  treatment  of  geometric  reasoning,  the  world  consists  of 
only  two-dimensional  rectilinear  polygonal  obstacles,  and  the  robot  is  modeled 
in  disc-shaped. 

2.  The  problem  should  be  broad  enough  in  scope 

Non-holonomic  and  kinematic  constraints  are  imposed  on  the  disc  robot  so 
that  the  study  result  could  apply  to  a  wide  range  of  rigid  body  robots  modeled 
in  disc-shaped.  The  study  result  should  also  be  amenable  to  extensions  to 
general  polygonal  world  and  non-disc-shaped  robots. 

3.  The  problem  should  pose  research  challenge 

The  desired  characteristics  of  the  planned  path,  which  calls  for  feasible  end 
portions  and  smooth  middle  portion,  are  issues  not  well-established  in  the 
Robotic  field.  Yet  they  are  realistic  requirements  of  paths  to  be  planned  in  an 
office-like  environment,  and  are  worth  the  research  effort  for  investigation. 

4.  The  problem  should  be  practical 

There  is  a  research  robot  Yamabico-11  under  continuous  development  by  stu¬ 
dents  and  faculty  at  the  Naval  Postgraduate  School.  Its  target  operating  envi¬ 
ronment  is  an  in-door  office  environment  with  mostly  rectilinear  type  of  con¬ 
structions  and  obstacles.  The  general  findings  of  this  thesis  should  be  able  to 
be  readily  tailored  for  implementation  on  the  Yamabico-11  robot  within  the 
context  of  its  target  operating  environment. 
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D.  IN  COMPARISON  WITH  THE  BASIC  MOTION  PLAN¬ 
NING  PROBLEM 


In  his  book  “Robot  Motion  Planning” [Ref.  1],  J.  C.  Latombe  defines  the  basic 
motion  planning  problem  (or  the  classical  path  planning  problem)  as  follows: 


Let  A  be  a  single  rigid  object  (the  robot )  moving  in  a  Euclidean  space  W, 
called  workspace,  represented  as  KN ,  with  N  =  2  or  3. 

Let  be  fixed  rigid  objects  distributed  in  W.  The  B,  ’s  in  W  are 

accurately  known.  Assume  further  that  no  kinematic  constraints  limit  the  motions 
of  A  (we  say  that  A  is  a  free-flying  object^. 

The  problem  is:  Given  an  initial  position  and  orientation  and  a  goal  position 
and  orientation  of  A  in  W,  generate  a  path  r  specifying  a  continuous  sequence  of 
positions  and  orientations  of  A  avoiding  contact  with  the  B{ ’s,  starting  at  the  ini¬ 
tial  position  and  orientation,  and  terminating  at  the  goal  position  and  orientation. 
Report  failure  if  no  such  path  exists. 


The  scope  of  this  thesis  is  a  simplification  of  this  basic  motion  planning  problem 
in  some  aspects,  and  an  extension  in  other  aspects: 

•  Simplifications: 

1.  The  robot  T>  is  modeled  as  a  disc  robot.  Because  of  its  geometry  sim¬ 
plicity,  when  V  is  represented  as  a  point  in  its  configuration  space  (C- 
s/»ace)[Ref.  2],  its  C- space  is  only  two-dimensional.  The  obstacles  in  the 
C-space  (called  the  C-obstacles)  are  those  obstacles  in  W  grown  by  a 
bumper  of  depth  r0  which  is  the  radius  of  V.  The  resulting  diagram  of 
the  C-obstacles  is  called  the  convolution  diagram[Re{.  3]  (see  Figure  1.2). 
In  contrast  to  disc  robots,  the  C-space  of  non-disc-shaped  rigid  body 
robot  is  always  three-dimensional  which  makes  motion  planning  a  more 
difficult  task. 

2.  The  world  W  is  only  of  R2,  and  consists  only  of  rectilinear  polygonal 
obstacles. 
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•  Extensions'. 


1.  The  robot  T>  is  non-holonomic  and  unable  to  rotate  about  its  center. 
Thus,  the  motion  path  is  required  to  have  tangential  continuity.  This 
extension  of  the  problem  has  been  extensively  studied. 

2.  The  robot  T>  is  subject  to  the  kinematic  constraint  of  Kmax.  An  example 
of  such  constraint  is  the  limited  steering  range  of  a  vehicle.  This  exten¬ 
sion  of  the  problem  has  been  extensively  studied  in  [Ref.  1 :  Chapter  9] 
[Ref.  4]  [Ref.  5]  [Ref.  6]  [Ref.  7]  [Ref.  8]  [Ref.  9]. 

3.  Except  at  the  initial  and  final  portions  of  the  motion  path  where  backing 
up  motions  are  allowed,  the  robot  V  is  only  allowed  to  move  forward  in 
a  non-stopping  manner  for  the  middle  portion  of  the  path  in  between. 
An  example  of  such  a  situation  is  moving  a  car  from  one  place  of  a  city 
to  another  without  having  to  stop  for  traffic  lights  or  pedestrians.  The 
forward-motion  part  of  this  extension  for  the  middle  portion  of  the  path 
has  been  studied  in  [Ref.  1 :  Chapter  9]  [Ref.  6] [Ref.  7]  [Ref.  8]  [Ref.  9]. 
However,  there  has  not  been  much  investigation  by  researchers  on  the 
non-stopping-motion  part  of  this  extension  which  calls  for  additional  con¬ 
tinuity  in  curvature. 

4.  The  robot  V  is  subject  to  an  additional  kinematic  constraint  of  kmaz 
depending  on  its  traveling  speed.  An  example  of  such  constraint  is  that  a 
non-stopping  vehicle  when  making  a  turn  is  subject  to  a  maximum  rate 
of  change  of  steering  angle.  This  extension  of  the  problem,  to  the  best  of 
the  authors’  knowledge,  has  not  been  investigated  before. 
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E.  RELATED  WORK 


In  the  basic  motion  planning  problem,  planning  a  collision-free  path  for  an 
unconstrained  disc  robot  V  in  the  free  space  of  W  (C  R2)  is  equivalent  to  planning 
a  connected  path  for  a  reduced  point-P  in  the  free  space  of  C-space.  By  all  accounts, 
the  basic  motion  planning  problem  for  unconstrained  V  (not  irregularly-shaped  rigid 
body  robot  in  general)  can  be  considered  solved,  particularly  under  the  cost  metrics  of 
“shortest  distance  [Ref.  3]”  or  “maximum  safety  clearance  [Ref.  10]”.  Note  that  in  the 
basic  motion  planning  problem,  since  V  is  not  subject  to  any  kinematic  constraints, 
it  can  stop  anywhere  and  change  its  orientation  at  the  same  spot  by  rotating  about 
its  center.  Therefore,  the  solution  path  can  be  in  arbitrary  shape  that  requires  no 
tangential  continuity.  The  initial  and  final  orientations  are  irrelevant.  Backing  up 
motions  are  not  necessary  at  all. 

The  basic  motion  planning  problem  for  V  becomes  more  interesting  and  chal¬ 
lenging  when  it  is  extended  by  imposing  real-life  kinematic  constraints  on  the  robot. 
Interests  in  the  extension  problem  have  been  intensified  after  Laumond  published 
his  results  in  1986  [Ref.  4].  In  his  paper,  Laumond  proved  that  in  the  case  of  a  point 
robot  with  KmaI  constraint,  for  every  two  configurations  connected  by  a  collision-free 
path  in  free(W)  C  R2,  there  exists  a  feasible  path  between  them  which  satisfies  the 
Kmax  constraint.  His  method  breaks  down  the  planning  problem  into  two  phases.  In 
a  first  phase,  the  problem  is  solved  as  a  basic  motion  planning  problem  in  that  an 
(arbitrary)  collision-free  path  is  found  ignoring  the  orientations  of  S  and  G.  In  a 
second  phase,  this  path  is  transformed  into  a  topologically  equivalent  collision-free 
path  using  only  Rmtn~ arcs  and  tangent  line  segments.  Unfortunately,  the  number  of 
maneuvers  (reversals)  introduced  in  the  feasible  path  constructed  by  his  proof  is  not 
bounded,  and  hence  the  computation  complexity  is  difficult  to  analyze.  Also,  a  path 
involving  reversals  throughout  its  course  are  not  smooth  and  are  required  only  in 
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very  constrained  environment.  This  algorithm  is  generally  regarded  as  having  high 
theoretical  value  but  little  practical  use. 

Another  feasible  path  planning  algorithm  presented  by  Vasseur  et  al.  [Ref.  5] 
takes  a  different  approach.  Reversals  are  still  allowed  throughout  the  path  for  a  non- 
holonomic  car-like  vehicle.  However,  for  path  planning,  they  break  up  free(W)  into 
convex  cells,  and  calculate  intermediate  configurations  at  the  border  of  every  adjacent 
pair  of  cells.  They  then  maneuver  the  vehicle  from  S  to  G  via  all  the  in-between 
intermediate  configurations.  Since  all  the  intermediate  configurations  of  a  convex 
cell  are  visible  to  one  another,  a  simple  procedure  is  devised  to  connect  them  using 
only  /Zmin-arcs  and  tangent  lines  (probably  after  some  numbers  of  reversals).  This 
method  seems  complete  but  the  authors  did  not  claim  so  explicitly  in  their  paper. 
The  computation  bound  was  also  not  discussed  because  the  number  of  reversals 
involved  may  be  unbounded.  The  simulation  results  showed  that  the  solution  path 
involves  a  lot  of  unnecessary  reversals  even  in  a  spacious  environment,  and  looks 
unnatural  and  awkward. 

On  the  other  hand,  there  are  difficulties  in  planning  near  smooth  paths  for 
V  without  reversals  (smooth  path  planning  has  not  been  well  investigated  by  re¬ 
searchers).  If  reversal  maneuvers  are  disallowed  in  a  path  all  together,  it  has  been 
shown  (see  [Ref.  1]  for  a  good  introduction)  that  a  forward  moving  rigid  body  robot 
(and  hence  V)  with  Kmax  constraint  is  not  fully  controllable.3  At  present,  it  is  still 
an  open  question  regarding  the  lower  bound  on  the  problem  of  finding  a  near  smooth 
(^max-constrained)  path  even  for  just  point  robots  [Ref.  11].  One  of  the  best  results 
for  point  robots  so  far  is  credited  to  G.  Wilfong  and  S.  Fortune.  In  [Ref.  6],  they 
gave  an  exponential  complexity  decision  procedure  for  this  problem,  although  their 
algorithm  does  not  find  the  actual  path. 

3A  robot  is  said  to  be  fully  controllable  if  and  only  if,  for  any  distribution  of  obstacles  in  the 
workspace,  if  there  exists  a  free  path  between  any  two  configurations  S  and  G,  then  there  also 
exists  a  feasible  path  between  these  two  configurations  [Ref.  1  :  Chapter  9], 
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Because  of  these  difficulties,  many  researchers  have  come  up  with  incomplete 
algorithms  to  plan  near  smooth  paths  for  forward-moving  rigid  body  robots  (and 
hence  V)  subject  to  Kmax  constraint.  They  normally  start  with  a  global  planning  that 
produces  a  network  of  “corridors”  (Ref.  8],  “lanes”  [Ref.  7]  or  “spines” [Ref.  9]  that 
are  extracted  from  free(W).  Local  planning  techniques  are  then  used  to  generate 
circular  turns  with  curvature  smaller  that  nmax  for  transferring  the  robot  from  a 
corridor  (or  lane  or  spine)  to  another  at  a  junction  of  the  network.  Invariably,  all  of 
them  avoided  the  end  portions  problem  by  conveniently  assuming  that  S  and  G  lie 
on  the  spines  and  point  to  one  of  the  two  directions  of  a  spine.  Also,  the  solution 
paths  are  only  near  smooth,  which  consist  of  straight  line  segments  and  circular  arcs. 
They  are  piecewise  of  class  C2,  but  are  of  class  C1  in  their  entirety.  At  the  junction 
of  a  line  segment  and  a  circular  arc,  the  curvature  is  not  continuous.  Thus,  a  non- 
holonomic  roboi  has  to  stop  at  each  junction  abruptly  in  order  to  follow  the  paths. 
This  problem  normally  arises  from  the  choice  of  circular  arcs  for  junction  turns,  but 
not  because  of  the  constrained  environment. 

From  the  author’s  point  of  view,  none  of  these  surveyed  algorithms  is  suitable  for 
planning  a  motion  path  for  T>  in  a  rectilinear  polygonal  world  resembling  an  in-door 
office  environment.  That  is,  none  of  them  is  a  satisfactory  answer  to  the  problem 
statement  set  forth  in  this  thesis.  Of  these  existing  results,  the  author  opines  that  the 
approach  of  the  incomplete  algorithms  which  plan  near  smooth  paths  for  forward- 
moving  robots  subject  to  Kmax  constraint  provides  a  most  useful  framework  for  this 
thesis  to  work  with.  Based  on  such  a  framework  and  compilation  of  other  related 
work,  this  thesis  further  investigates  the  following  open  issues: 

1.  Layered  Motion  Planning  Approach 

Based  on  the  initial  idea  of  breaking  down  the  motion  planning  problem  into 
phases,  a  formalized  layered  motion  planning  approach  is  fully  explored. 
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2.  Sensor-Oriented  Global  Path  Planning  Method 

Of  the  existing  global  path  planning  methods,  none  is  suitable  for  robots  which 
desire  to  follow  parallel  walls  for  position  and  orientation  correction  using  sonar 
returns.  A  novel  sensor-oriented  method  is  proposed  in  this  thesis. 

3.  Local  Smooth  Path  Planning 

To  plan  for  smooth  paths  instead  of  near  smooth  ones,  a  class  of  curves  called 
cubic  spirals  [Ref.  12]  instead  of  circular  arcs  are  used  to  generate  turns.  The 
properties  of  cubic  spirals  under  the  kinematic  constraints  are  thoroughly  in¬ 
vestigated. 

4.  End  Portions  Path  Planning 

To  the  best  of  the  authors’  knowledge,  this  problem  has  not  been  investigated 
before;  Related  studies  in  this  area  as  discussed  above  are  not  directly  relevant 
and  applicable  to  this  paper  either. 

F.  METHODOLOGY 

One  contribution  of  this  thesis  is  the  laying  down  of  a  formal  framework  of  lay¬ 
ered  motion  planning  approach  to  tackle  the  full  problem  of  motion  planning.  Pre¬ 
liminary  attempts  of  such  an  approach  can  be  found  in  [Ref.  6] [Ref.  7]  [Ref.  8]  [Ref.  9]. 
In  this  thesis,  the  task  of  motion  planning  is  formally  broken  down  into  the  following 
four  layers: 

1.  Global  path  planning  layer  (preprocessing  step). 

2.  Local  smooth  path  planning  layer  (preprocessing  step). 

3.  End  portions  path  planning  layer  (per  query). 

4.  Graph  search  layer  (per  query). 
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In  [Ref.  1 :  Chapter  4],  Latombe  groups  a  large  number  of  methods  for  solving 
the  basic  motion  planning  problem  into  the  following  three  broad  categories  and 
sub-categories: 

1.  Roadmap  Approach 

(a)  Visibility  Graphs 

(b)  Voronoi  Diagrams 

(c)  Freeway  Nets 

(d)  Silhouettes 

2.  Cell  Decomposition  Approach 

(a)  Exact  Methods 

(b)  Approximate  Methods 

3.  Potential  Field  Approach 

This  thesis  adopts  only  the  roadmap  approach  for  the  global  path  planning 
layer.  For  the  local  smooth  path  planning  layer,  this  thesis  adopts  a  class  of  smooth 
curves  called  cubic  spirals  [Ref.  12]  to  design  paths  of  curvature  continuity.  The  end 
portions  path  planning  layer  represents  an  original  effort  of  this  thesis.  The  graph 
search  layer  makes  a  Dijkstra’s  search  of  the  roadmap  to  find  an  optimal  solution  for 
the  motion  planning  problem. 
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G.  ORGANIZATION  OF  STUDY 


The  layered  motion  planning  approach,  which  divides  the  problem  statement  of 
this  thesis  into  four  layers  and  conquers  them  separately,  is  explained  in  Chapter  II. 
This  should  give  the  reader  a  good  overview  of  the  approach  before  each  layer  is 
examined  in  detail.  Chapter  III  explores  a  number  of  roadmap  methods  that  can  be 
employed  in  the  global  path  planning  layer.  A  novel  sensor-oriented  roadmap  method 
is  introduced.  Chapter  IV  is  devoted  to  a  detailed  investigation  on  the  properties  of  a 
class  of  C2  curves  called  cubic  spirals  which  will  be  used  in  the  subsequent  chapters 
for  planning  smooth  turns.  Chapter  V  investigates  the  techniques  of  generating 
smooth  cubic  spiral  turns  for  the  global  roadmaps  in  the  local  smooth  path  planning 
layer.  Chapter  VI  proposes  a  method  for  planning  the  end  portions  of  paths  in 
smooth  roadmaps.  Chapter  VII  uses  Dijkstra’s  Search  in  the  graph  search  layer  to 
select  an  optimal  solution  path  out  of  a  number  of  candidates  based  on  some  cost 
functions.  At  this  point,  an  answer  to  the  problem  statement  of  this  thesis  will  have 
been  offered.  Chapter  VIII  offers  some  thoughts  on  how  the  results  of  this  thesis 
could  be  tailored  for  implementation  on  the  Yamabico-11  robot.  Finally,  conclusions 
and  avenues  for  future  research  are  presented  in  Chapter  IX. 
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II.  LAYERED  MOTION  PLANNING 

APPROACH 


As  discussed  in  Chapter  I,  the  task  of  motion  planning  is  a  formidable  under¬ 
taking.  Even  in  the  scaled  down  problem  of  motion  planning  for  a  forward-moving 
point  robot  subject  only  to  the  /cmar  constraint  and  the  C1  requirement,  there  is  still 
no  known  exact  algorithm  for  a  solution.  Many  researchers  thus  resort  to  incomplete 
but  practical  algorithms  by  breaking  similar  scaled  down  problems  into  two  layers: 
global  path  planning  and  local  path  planning. 

In  this  thesis,  the  problem  statement  is  considerably  more  involved.  It  could 
be  conceived  that  any  attempt  for  an  exact  and  compl''-  lutiop  would  be  unman¬ 
ageable  within  the  allowable  resources  and  schedule  of  this  study.  Therefore,  the 
same  line  of  thinking  by  attacking  the  full  problem  statement  via  layered  motion 
planning  approach  is  adopted.  The  task  of  motion  planning  is  formally  divided  into 
the  following  four  layers: 

1.  Global  path  planning  layer  (preprocessing  step). 

2.  Local  smooth  path  planning  layer  (preprocessing  step). 

3.  End  portions  path  planning  layer  (per  query). 

4.  Grapli  search  layer  (per  query). 

Beside^  dividing  and  conquering  the  motion  planning  problem,  this  layered 
motion  planning  approach  offers  another  advantage  of  modularity.  The  same  class 
of  roadmaps  generated  from  the  global  path  planning  layer  can  be  smoothed  out  into 
C2  roadmaps  using  a  common  technique  of  the  local  smooth  path  planning  layer. 
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A  common  end  portions  path  planning  method  can  be  applied  to  the  same  class  of 
smooth  C2  roadmaps.  Finally,  a  common  graph  search  technique  can  be  applied  to 
select  an  optimal  solution  path  out  of  a  number  of  candidates  based  on  different  cost 
functions  defined  for  each  type  of  roadmap.  Figure  2.1  depicts  such  a  modularity 
concept. 

A.  GLOBAL  PATH  PLANNING  LAYER  (PREPROCESS¬ 
ING  STEP) 

Given  W ,  the  goal  of  this  preprocessing  step  is  to  capture  the  connectivity  of 
free(W)  in  the  form  of  a  network  of  one-dimensional  curves  lying  in  free(W).  This 
network  of  one-dimensional  curves  is  called  a  roadmap.  Various  methods  based  on 
different  principles  have  been  proposed,  producing  various  sorts  of  roadmaps,  called 
“visibility  graphs”,  “freeway  nets”,  “Voronoi  diagrams”,  and  “silhouettes” [Ref.  1:  Ch. 

4]- 

In  this  thesis,  roadmaps  are  classified  according  to  their  geometric  properties, 
not  the  methods  from  which  they  are  generated.  A  roadmap  is  represented  as  M.  = 
(V,£),  where  V  is  the  collection  of  all  vertices,  and  £  the  collection  of  all  edges. 
Roadmaps  of  the  same  class  will  have  similar  basic  data  structure  representation 
for  M.  At  present,  three  kinds  of  roadmaps  have  been  classified  in  the  global  path 
planning  layer: 

1.  Visibility  Net 

This  is  a  graph  consists  of  circular  arcs  and  common  tangent  line  segments.  The 
vertices  are  the  tangent  points.  The  edges  are  the  directed  circular  arcs  and 
the  directed  tangent  line  segments.  An  example  of  the  method  that  generates 
this  kind  of  roadmap  is  the  well-known  visibility  graph  method. 
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2.  Nominal  Configuration  Net 

This  is  a  graph  consists  of  logically  interconnecting  nominal  configurations. 
The  vertices  are  the  nominal  configurations.  The  edges  are  the  logical  con¬ 
nections  between  some  pairs  of  the  vertices.  An  example  of  the  method  that 
generates  this  kind  of  roadmap  is  a  novel  minimum  gate  method  proposed  in 
this  thesis. 

3.  Spine  Net 

This  is  a  graph  consists  of  only  straight  line  segments  called  spines.  The  vertices 
are  the  directed  turns  at  the  intersection  junctions  between  two  directed  spines. 
The  edges  are  the  sections  of  directed  spines  connecting  two  directed  turns.  An 
example  of  the  method  that  generates  this  kind  of  roadmap  is  the  well-known 
freeway  net  method.  A  novel  sensor-oriented  method  is  also  proposed  in  this 
thesis. 

B.  LOCAL  SMOOTH  PATH  PLANNING  LAYER  (PRE¬ 
PROCESSING  STEP) 

Given  a  roadmap  M  of  free(W),  the  goal  of  this  preprocessing  step  is  to 
generate  collision-free  cubic  spirals  satisfying  the  Kmax  and  kmaI  constraints  for  all 
turns  in  the  roadmap.  Since  not  all  turns  are  possible  to  be  realised  by  constrained 
cubic  spirals,  those  impossible  vertices  and  edges  are  removed  from  «M.  The  resulting 
smoothed  out  roadmap  is  called  a  smooth  roadmap. 

Note  that  since  straight  line  segments  are  just  special  cases  of  cubic  spirals,  the 
smooth  roadmap  is  said  to  consist  of  only  cubic  spiral  segments.  Any  path  in  the 
smooth  roadmap  is  a  smooth  path  of  class  C2. 
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C.  END  PORTIONS  PATH  PLANNING  LAYER  (PER 
QUERY) 


A  query  is  a  request  to  plan  a  smooth  path  for  an  initial  configuration  S  and 
a  final  configuration  G  in  frce(W).  In  this  layer,  the  problem  of  motion  planning 
is  reduced  to  finding  some  candidates  of  intermediate  So  (can  be  coincident  with  S) 
and  intermediate  Go  (can  be  coincident  with  G)  such  that: 

1.  It  is  possible  to  plan  a  feasible  path  from  S  to  So. 

2.  It  is  possible  to  merge  So  into  the  smooth  M.  using  only  cubic  spirals. 

3.  It  is  possible  to  diverge  from  the  smooth  M.  into  Go  using  only  cubic  spirals. 

4.  It  is  possible  to  plan  a  feasible  path  from  Go  to  G. 

At  the  end  of  this  layer,  a  number  of  candidate  intermediate  configurations  of  So 
and  G0  would  have  been  found. 

D.  GRAPH  SEARCH  LAYER  (PER  QUERY) 


In  this  layer,  a  Dijkstra’s  search  is  made  for  each  pair  of  the  candidate  So  and 
Go  on  the  smooth  M..  A  number  of  candidate  solution  paths  which  meet  the  path 
characteristics  as  set  forth  in  the  problem  statement  will  be  produced.  An  optimal 
solution  path  from  S  to  So  to  Go  to  G  will  be  selected  out  of  these  candidate  solution 
paths  based  on  some  cost  functions.  Factors  that  can  be  considered  in  a  cost  function 
include: 


•  path  length 

•  path  smoothness 

•  safety 
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•  number  of  turns 


•  number  of  reversals 

•  wall  following  distance 

Actual  definition  of  a  cost  function  is  not  important  in  theoretical  studies.  However, 
in  actual  applications,  specific  cost  functions  must  be  defined  for  each  specific  case. 

E.  REMARKS 


In  a  cramped  and  cluttered  environment  with  narrow  passages,  this  layered  mo¬ 
tion  planning  approach  may  fail  to  find  a  solution  even  if  one  exists.  However,  this 
incompleteness  should  not  be  a  serious  problem  because  the  target  operating  envi¬ 
ronment  for  the  disc  robot  T>  consists  of  only  rectilinear  polygonal  obstacles  which 
represents  realistics  office  environment  with  ample  leeway  for  maneuvering.  Where 
this  approach  fails  to  find  a  solution  path,  the  environment  would  have  been  too  con¬ 
strained  even  in  the  eyes  of  human  beings.  More  importantly,  this  approach  allows 
feist  computation  and  satisfactory  results  of  smooth  paths,  and  makes  it  possible  for 
onboard  implementation  in  the  computer  system  of  V. 
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III.  GLOBAL  PATH  PLANNING  LAYER 


A.  OVERVIEW 

The  Global  path  planning  layer  is  the  first  layer  in  the  layered  motion  planning 
approach  proposed  in  this  thesis.  The  goal  of  this  layer  is  to  capture  the  connectivity 
of  frec(W)  in  the  form  of  a  network  of  one-dimensional  curves  lying  in  free(W). 
Thus  reducing  the  two-dimensional  motion  planning  problem  to  one  that  is  basically 
a  one-dimensional  graph  search  problem.  This  network  of  one-dimensional  curves 
is  called  a  roadmap.  Given  W ,  this  layer  is  a  preprocessing  step  that  needs  to  be 
carried  out  only  once  to  serve  all  subsequent  on-line  queries  of  motion  planning. 

Various  methods  based  on  different  principles  have  been  proposed,  producing 
various  sorts  of  roadmaps.  In  this  thesis,  roadmaps  are  classified  according  to  their 
geometric  properties,  not  the  methods  from  which  they  are  generated.  A  roadmap 
is  represented  a s  M  =  (V,£),  where  V  is  the  collection  of  all  vertices,  and  £  the 
collection  of  all  edges.  Roadmaps  in  the  same  class  will  have  similar  basic  data 
structure  representation  for  M.  At  present,  three  kinds  of  roadmaps  have  been 
classified  in  *he  global  path  planning  layer,  namely,  the  visibility  net,  the  nominal 
configuration  net  and  the  spine  net. 

In  the  following  sections,  each  class  of  roadmaps  will  be  discussed  in  detail. 
Discussions  will  focus  on  the  various  methods  that  can  be  used  to  generate  the  same 
class  of  roadmap,  time  complexity  analyses,  and  a  sketch  of  the  basic  data  structure 
representation  for  the  roadmap  M. 
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B.  THE  VISIBILITY  NET 


1.  Introduction 

This  is  a  graph  consists  of  circular  arcs  and  common  tangent  line  segments. 
The  vertices  in  the  visibility  net  are  all  the  tangent  points  where  the  tangent  line 
segments  touch  the  circular  arcs.  The  edges  in  the  net  are  the  directed  circular  arcs 
and  the  directed  tangent  line  segments.  The  only  method  that  generates  this  kind 
of  roadmap  is  the  well-known  visibility  graph  method. 

2.  The  Visibility  Graph  Method 
a.  Background 

Planning  the  shortest  path  for  a  disc  robot  P  using  visibility  graph 
method  for  the  basic  motion  planning  problem  is  a  well-known  problem.  Chew 
[Ref.  3]  presented  a  complete  algorithm  solving  the  problem  in  O(n2log  n)  time  where 
n  is  the  number  of  edges  that  make  up  the  polygonal  obstacles  in  W.  However,  when 
P  is  subject  to  /cmox  constraint,  finding  shortest  near  smooth  patht  amongst  obstacles 
is  still  an  open  question.  Even  if  the  “shortest-path”  requirement  is  dropped,  no  one 
has  yet  invented  a  polynomial  time  complete  procedure  to  find  near  smooth  paths 
for  P.  Let  alone  the  kmax  constraint  for  smooth  paths. 

Nonetheless,  the  visibility  graph  method  can  be  employed  in  the  global 
path  planning  layer  to  come  up  with  a  roadmap  M  (visibility  net)  that  lies  com¬ 
pletely  in  free(W).  Subsequently,  in  the  local  smooth  path  planning  layer,  attempts 
could  be  made  to  modify  Ai  into  smooth  At  by  replacing  the  circular  arcs  in  Ai  by 
cubic  spirals. 


23 


b.  Details  of  the  Visibility  Graph  Method 

The  following  procedure  which  builds  the  visibility  net  M  =  (V,£)  for 
T>  in  the  basic  motion  planning  problem  is  extracted  from  the  work  of  Chew[Ref.  3]. 

Procedure  Build  Visibility  Net: 

1.  Step  1  Construct  the  convolution  diagram 

First  construct  the  convolution  diagram  of  the  configuration  space  (C-space) 
for  T>  so  that  D  can  be  represented  as  a  point.  The  obstacles  in  the  C- 
space  (called  the  C-obstacles)  are  those  obstacles  in  W  grown  by  a  bumper 
of  depth  r0  which  is  the  radius  of  T>.  We  call  the  resulting  diagram  of  the  C- 
obstacles  the  convolution  diagram  (see  Figure  1.2).  The  convolution  diagram 
can  be  constructed  in  0(n2logn)  time  using  simple  ray  sweep  technique,  or  in 
0(n  log  n)  time  using  the  more  complex  Voronoi  diagram  as  a  preprocessing 
step. 

2.  Step  2  Build  the  visibility  net 

For  each  circular  vertex  in  the  convolution  diagram,  apply  the  ray  sweep  tech¬ 
nique  on  the  tangent  ray  emanating  from  it  to  determine  all  other  circular 
vertices  that  are  visible.  This  step  takes  a  total  of  0(n2  log  n)  to  build  the 
visibility  net. 

3.  Data  Structure  of  the  Visibility  Net 

Suppose  there  are  n  collision-free  circular  vertices  in  the  convolution  di¬ 
agram.  Between  a  pair  of  collision-free  circular  vertices,  there  are  at  most  eight 
common  directed  tangent  edges  and  eight  directed  tangent  contacts  (a  directed  tan¬ 
gent  contact  is  the  directed  point  where  a  tangent  edge  touches  a  circular  vertex). 
Therefore,  there  are  at  most  0(n2)  directed  tangent  edges  and  0(n2)  directed  tan¬ 
gent  contacts  in  the  convolution  diagram. 
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The  vertices  of  the  visibility  net  M  are  the  “visible  directed  tangent  con¬ 
tacts”  lying  on  the  exterior  portion  of  the  circular  vertices.1  The  edges  are  the 
“visible  directed  tangent  edges”  connecting  two  vertices  from  different  circular  ver¬ 
tices,  and  the  “visible  directed  circular  arcs”  connecting  two  vertices  on  the  same 
circular  vertex.  See  Figure  3.1  for  an  illustration. 


Figure  3.1:  Vertices  and  Edges  of  the  Visibility  Net  M 


'A  directed  tangent  contact  is  “invisible”  if  it  lies  on  the  interior  potion  of  the  circular  vertex 
which  is  inside  a  C-obstacle. 
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4.  Time  Complexity  Analysis  of  Building  the  Visibility 
Net 

Assuming  there  are  n  obstacle  edges  in  W .  Building  the  convolution  di¬ 
agram  takes  0(n2logn)  time  using  simple  ray  sweep  technique,  or  O(nlogn)  time 
using  Voronoi  diagram  as  a  preprocessing  step.  Constructing  the  visibility  net  takes 
0(n 2  logn)  time. 

Conclusion  Building  the  visibility  net  in  the  global  path  planning  layer 
takes  0(n2logn)  time. 

5.  Critique 

It  will  be  shown  in  Chapter  V  (local  smooth  path  planning  layer)  that 
the  visibility  net  M.  buib  f  om  Procedure  Build  Visibility  Net  of  this  section  is  not 
amenable  to  smooth  in  ~  techniques.  Rather,  this  procedure  must  be  modified  to  take 
into  account  kinematic  constraints  faced  by  V.  Because  the  modification  involves 
understanding  of  the  kinematic  constraints  imposed  on  the  cubic  spirals,  the  discus¬ 
sion  of  this  issue  will  be  deferred  until  Chapter  V  after  the  properties  of  the  cubic 
spirals  have  been  thoroughly  investigated  in  Chapter  IV.  The  fact  that  the  kinematic 
constraints  of  T>  must  be  taken  into  account  in  the  global  path  planning  layer  makes 
the  visibility  graph  method  inferior  to  other  methods. 
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C.  THE  NOMINAL  CONFIGURATION  NET 

1.  Introduction 

This  is  a  graph  consists  of  logically  interconnecting  nominal  configurations. 
The  vertices  are  all  the  nominal  configurations.  The  edges  are  logical  connections 
between  some  pairs  of  the  vertices.  A  novel  minimum  gate  method  is  proposed  in 
this  thesis  to  generate  this  kind  of  roadmap. 

2.  The  Minimum  Gate  Method 

a.  Background 

The  minimum  gate  method  is  a  novel  concept  that  stems  from  the 
concept  of  Voronoi  diagram.  The  intuition  behind  this  new  method  is  the  observation 
that  minimum  gates  (or  bottlenecks)  of  a  path  occur  at  those  points  where  the  safety 
clearance  is  minimal.  If  the  disc  robot  V  cannot  make  it  through  a  minimum  gate, 
the  whole  path  is  infeasible.  Therefore,  it  may  be  more  efficient  and  intuitively 
appealing  to  focus  the  planning  of  a  global  roadmap  around  these  minimum  gates. 

However,  as  stated  in  the  Introduction  Chapter,  some  naive  methods 
can  produce  apparently  impressive  results,  but  the  limitations  of  these  methods  quickly 
become  obvious.  This  novel  method  is  a  good  demonstration  of  this  statement.  It 
will  be  shown  later  that  while  this  method  is  elegant  and  efficient  in  some  situations, 
it  quickly  fails  in  some  non-trivial  counter-examples. 

In  the  following  discussion  of  this  novel  method,  formal  definitions  are 
first  given  for  the  minimum  gate  and  the  nominal  configuration,  followed  by  a  discus¬ 
sion  of  how  to  extract  them  from  W ,  and  how  to  construct  a  nominal  configuration 
net  out  of  them. 
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b.  Definitions 


The  following  notations  are  adopted  for  the  subsequent  definitions: 

•  Let  uor(fVr)  denote  the  Voronoi  diagram  of  W.  It  consists  of  straight  and 
parabolic  arcs. 

•  Denote  a  straight  arc  of  vor(\V)  by  e,  and  a  parabolic  arc  by  c. 

•  Denote  the  two  end  points  of  a  Voronoi  arc  by  f,(e),  t2(e),  ti(c)  and  <2(c),  etc. 

•  Denote  by  p(e)  and  p(c)  any  point  on  e  and  c  respectively,  and  we  say  p(e)  £  e 
and  p(c)  £  c. 

•  Denote  by  te[p(e)]  and  w\p(c)]  the  clearance  of  p(e)  or  p(c)  from  the  nearest 
obstacle  in  W. 

A  spine  is  an  e  €  uor(VV),  where  Vpi(e),p2(e)  6  e,w\pi(e)]  =  ze[p2(e)]. 
See  Figure  3.2(a). 

A  skew  is  an  e  €  vor(W),  where  3pi(e),p2(e)  £  e,  such  that  tefp^e)]  ^ 
ie[p2(e)].  See  Figure  3.2(b). 

A  twisted  pair  is  a  pair  of  consecutive  Ci,c2  €  vor(W),  where  <2(^1)  = 
<i(c2),  and  u^/i^)]  >  tx7[<2(c! )]  and  te[fj(c2)]  <  ie[f2(c2)].  A  twisted  pair  occurs  at 
two  opposite  corners  lying  on  opposite  sides  of  a  line.  See  Figure  3.2(c). 

A  local  minimum  point  is  either  the  point  on  a  skew’  that  is  closest  to 
the  two  bisected  corners,  or  the  minimum  point  on  a  twisted  pair  (which  is  also  the 
closest  point  bisecting  two  corners). 

A  minimum  gate  is  either  an  end  point  of  a  spine,  or  a  local  mini¬ 
mum  point.  More  specifically,  let  p(V)  denote  the  set  of  minimum  gates.  Then  the 
minimum  gates  are  the  following  points  on  uor(VF): 
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1.  For  an  c  G  vor(W),  if  Vp!(e),p2(e)  6  e,u;[pi(e)]  =  u;[p2(e)],  then  *i(e),*2(e)  6 
p(V).  See  Figure  3.2(a). 

2.  For  an  e  €  vor(W ),  if  3p(e)  6  e,  such  that  Vp3(e)  €  e,  p3(e)  ^  p(e)  4=>  u>[p(e)|  < 
u>[p3(e)],  then  p(e)  €  p(V).  See  Figure  3.2(b). 

3.  For  every  two  consecutive  Ci  and  c2  withf2(ci)  =  ti(c2),  if  w[<i(ci)]  >  te[f2(ci)] 
and  u>[<i(c2)]  <  te[f2(c2)],  then  f2(ci)  €  p(V).  See  Figure  3.2(c). 

Each  minimum  gate  can  be  assigned  two  tangential  orientations,  9  and 
9  +  180°.  That  is,  each  minimum  gate  defines  two  nominal  configurations.  See 
Figure  3.3.  The  set  of  nominal  configurations  is  denoted  by  V,  which  is  also  the  set 
of  vertices  of  the  nominal  configuration  net  M.. 

c.  Details  of  the  Minimum  Gate  Method 

The  nominal  configuration  net  M  can  be  constructed  as  follow’s: 
Procedure  Build  Nominal  Configuration  Net: 

1.  Step  1  Extract  the  Nominal  Configurations 

From  the  definitions  of  minimum  gates  and  nominal  configurations,  we  can  see 
that  it  is  straight  forward  to  extract  nominal  configurations  from  vor(W).  See 
Figure  3.3. 

2.  Step  2  Build  the  Nominal  Configurations  Net  M. 

Let  M.  =  (V,£),  where  V  is  the  set  of  nominal  configurations.  S  can  be 
constructed  as  follows: 

(a)  Two  minimum  gates  are  connected  if  there  is  a  path  in  vor{W)  connecting 
them.  From  two  connected  minimum  gates,  we  can  generate  two  edges 
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Figure  3.3:  Nominal  Configurations 
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Figure  3.4:  Vertices  and  Edges  in  Nominal  Configuration  Net 

(directed  paths)  connecting  two  vertices  (nominal  configurations).  See 
Figure  3.4(a)  and  3.4(b) 

(b)  Consider  the  case  where  a  minimum  gate,  say  t>t,  has  a  degree  of  three 
(see  uj  of  Figure  3.5  for  an  example).2  Supposing  uj  is  connected  to  t>2, 
t>3  and  v4.  If  U3  and  u4  are  not  connected  to  Vi  by  spines,  it  is  necessary 
to  introduce  an  additional  pseudo  connection  between  u3  and  vA.  As  a 
result,  we  introduce  two  additional  edges  in  E.  This  step  seems  strange, 
but  is  meant  to  resolve  an  inherent  difficulty  of  this  method  as  illustrated 
in  Figure  3.5. 

3.  Data  Structure  of  the  Nominal  Configuration  Net 

The  vertices  of  the  nominal  configuration  net  M  are  the  nominal  config¬ 
urations.  The  edges  are  the  logical  connections  between  nominal  configurations. 
Note  that  in  contrast  to  the  vi~ib>':  ty  graph  method  and  the  spine  net  method  to 
be  discussed  shortly,  the  edges  in  the  nominal  configuration  net  are  not  physical 
collision -free  paths,  but  are  just  logical  concepts.  The  construction  of  the  physical 
collision-free  paths  are  left  to  the  next  layer,  the  local  smooth  path  planning  layer. 

2There  is  a  common  assumed  property  of  Voronoi  diagram  that  the  maximum  degree  of  a  vertex 
is  three. 


Figure  3.5:  Introducing  Additional  Edges 

4.  Time  Complexity  Analysis  of  Building  the  Nominal 
Configuration  Net 

Assuming  there  are  n  obstacle  edges  in  IF.  Building  the  Voronoi  diagram 
as  a  preprocessing  step  takes  0{n  log  n)  time.  Extracting  the  nominal  configurations 
from  the  Voronoi  diagram  takes  linear  time.  The  number  of  vertices  (nominal  con¬ 
figurations)  in  a  nominal  configuration  net  is  in  the  order  of  0(n).  Assuming  that 
the  maximum  degree  of  a  Voronoi  diagram  vertex  is  three,  the  edges  in  a  nominal 
configuration  net  can  be  constructed  in  0(n)  time. 

Conclusion  Building  the  nominal  configuration  net  in  the  global  path 
planning  layer  takes  O(nlogn)  time. 
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D.  THE  SPINE  NET 

1.  Introduction 

This  is  a  graph  consists  of  only  straight  line  segments  called  spines.  Two 
methods  that  generate  this  kind  of  roadmap  are  examined  here.  One  is  the  well- 
known  freeway  method.  The  other  is  a  novel  sensor-oriented  method  proposed  in 
this  thesis.  As  a  matter  of  fact,  in  the  absence  of  a  good  algorithm  to  generate  spine 
net  automatically,  some  recent  papers  [Ref.  7]  [Ref.  9]  even  based  on  hand-drawn 
spine  nets  as  the  starting  point  of  investigations  into  path  planning  subject  to  nmax 
constraint.  It  seems  to  suggest  that  the  spine  net  has  a  natural  appeal  to  layered 
path  planning  approach,  and  is  more  manageable  when  kinematic  constraints  are 
imposed  on  the  solution  path. 

2.  The  Freeway  Method 

a.  Background 

The  freeway  method  was  first  investigated  by  Brooks[Ref.  13]  in  1983 
as  an  empirical  method  to  solve  the  basic  motion  planning  problem.  The  intuition 
behind  the  method  is  similar  to  that  of  a  Voronoi  diagram,  that  is,  keep  the  robot 
as  far  away  as  possible  from  the  obstacles.  But  unlike  Voronoi  diagrams  which 
require  complex  (but  complete)  method  to  construct  and  which  consist  of  parabolic 
segments,  the  freeway  method  uses  simple  and  intuitively  appealing  (but  incomplete) 
method  to  extract  only  straight-line  freeway  spines  from  W . 

This  method  is  known  to  be  incomplete,  and  hence  may  not  always 
find  a  free  path  for  the  basic  motion  planning  problem  even  if  one  exists.  However, 
experiments  with  the  freeway  method  have  shown  that  it  works  feist  in  a  relatively 
uncluttered  workspace[Ref.  13]. 
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b.  Details  of  the  Freeway  Method 

The  spine  net  can  be  constructed  using  the  freeway  method  as  follows:3 

Procedure  Build  Spine  Net  (Freeway  Method): 

1.  Step  1  Extract  the  Freeway  Spines 

(a)  For  every  pair  of  obstacle  edges  ei  and  e2  facing  each  other,  construct 
their  bisector  b. 

(b)  Extend  b  outwards  until  it  hits  some  other  obstacles.  See  Figure  3.6(a). 

(c)  Project  the  end  points  of  e\  and  e 2  normally  onto  the  bisector  b.  See 
Figure  3.6(b). 

(d)  Discard  those  intervals  of  b  which  do  not  include  at  least  a  vertex  from 
each  of  e\  and  e 2.  See  Figure  3.6(c). 

(e)  Discard  the  portions  of  b  which  have  clearance  smaller  than  the  radius  of 
T>.  That  is,  those  portions  which  are  not  collision-free.  See  Figure  3.6(d). 

(f)  The  remaining  intervals  of  6  are  the  freeway  spines  between  ex  and  e2. 
There  are  at  most  two  intervals  per  freeway  spine. 

2.  Step  2  Build  the  Spine  Net  M 

(a)  Compute  the  junctions  (intersections)  among  the  collection  of  freeway 
spines  (pairwise)  obtained  from  the  last  step. 

(b)  Build  the  spine  net  M.  =  (V,£),  where  V  is  the  collection  of  all  the 
directed  turns  (<  180°),  and  £  the  collection  of  the  directed  freeway 
spines. 

The  resulting  spine  net  M  is  reminiscent  of  a  Voronoi  diagram.  How¬ 
ever,  it  consists  of  only  rectilinear  lines  and  lines  of  orientations  ±45°.  Note  that 

3This  is  a  variant  of  Brook’s  method  for  the  basic  motion  planning  problem. 
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each  spine  has  two  directions,  6  and  0  +  180°.  Every  pair  of  intersecting  spines  gen¬ 
erates  four  pairs  of  intersecting  directed  spines  and  eight  distinct  directed  turns  at 
each  junction  (see  Figure  3.7). 
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Figure  3.6:  Construction  of  Freeway  Spines 


Figure  3.7:  Intersecting  Directed  Spines  and  Directed  Turns 
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3.  Th  Sensor-Oriented  Method 


a.  Background 

The  sensor-oriented  method  is  a  novel  method  proposed  in  this  thesis  to 
extract  only  rectilinear  straight-line  segments  called  spines  from  W .  Intuitively,  each 
spine  is  either  situated  at  a  suitable  distance  Wo  within  within  sensor  (for  example 
sonar)  range  and  parallel  to  an  obstacle  wall,  or  is  the  bisector  of  two  obstacle  walls 
facing  each  other  at  a  distance  <  2w0  and  >  2 r0,  where  r0  is  the  radius  of  the  disc 
robot  V. 

This  method  is  based  purely  on  the  following  observations  and  justifi¬ 
cations: 

•  For  a  disc  robot  T>  that  uses  sonar  sensors  to  correct  its  position  and  orientation 
errors,  it  is  important  that  V  follows  the  obstacle  walls  within  the  sonar  range 
as  far  as  possible. 

Supposing  the  sonar  range  is  [u;min,i(;mor],  where  wmtn  >  r0.  w0  is  chosen  to  be 
the  middle  value  of  the  sonar  range  for  open  walls,  so  that  there  are  maximum 
error  correction  ranges  at  both  sides  of  u;0.  When  two  facing  walls  are  closer 
than  2u>0,  it  is  still  all  right  for  T>  to  follow  the  bisector  line  so  long  as  it  is 
wide  enough  for  V  to  squeeze  through.  However,  the  narrower  the  passage, 
the  more  frequent  the  positional  correction  is  required  to  carry  out  in  order 
to  avoid  collision  because  of  accumulated  positional  errors.  When  two  facing 
walls  are  closer  than  2u>min,  following  the  bisector  may  be  possible  but  the 
sonar  will  be  out  of  range. 

•  In  order  to  avoid  becoming  a  wall-following  algorithm,  this  method  extends  the 
two  ends  of  each  spine  outwards  for  connections  with  other  spines.  This  allows 
for  a  choice  between  following  walls  at  a  greater  distance,  or  taking  shortcut 


37 


without  a  wall  in  range  for  position  and  orientation  correction.  The  choice  will 
be  based  on  some  cost  functions  to  be  discussed  later  in  the  graph  search  layer. 

b.  Details  of  the  Sensor-Oriented  Method 

The  spine  net  M.  can  be  constructed  using  the  sensor-oriented  method 
with  the  following  procedure: 

Procedure  Build  Spine  Net  (Sensor-Oriented  Method): 

1.  Step  1  Extract  the  Spines 

(a)  For  every  obstacle  wall  e,  construct  a  spine  (line  segment)  parallel  to  it 
at  a  distance  w0. 

(b)  Extend  each  spine  outwards  until  it  hits  some  other  obstacles. 

(c)  For  every  pair  of  obstacle  walls  ex  and  e 2  with  the  same  orientation  (either 
horizontal  or  vertical),  if: 

i.  their  distance  is  <  2 r0,  discard  those  portions  of  the  spines  of  ex  and 
ti  within  the  common  range. 

ii.  their  distance  is  >  2r0  but  <  ro  +  Wo,  replace  those  portions  of  the 
spines  of  ex  and  ej  within  the  common  range  by  the  bisector  of  the  two 
walls.  Extend  the  bisector  outwards  until  it  hits  some  other  obstacles 
or  comes  near  to  a  parallel  wall  within  the  distance  of  r0. 

iii.  their  distance  is  >  ro  4-  tvo  but  <  Wmin  +  introduce  an  additional 
spine  which  is  the  bisector  of  the  two  walls.  Extend  the  bisector 
outwards  until  it  hits  some  other  obstacles  or  comes  near  to  a  parallel 
wall  within  the  distance  of  r0.  See  Figure  3.8(a). 

(d)  Truncate  the  two  ends  of  each  spine  by  a  length  of  r0,  that  is,  those 
portions  which  are  not  collision-free.  See  Figure  3.6(b). 
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2.  Step  2  Build  the  Spine  Net  M 


(a)  Compute  the  junctions  (intersections)  among  the  collection  of  freeways 
(pairwise)  obtained  from  the  last  step. 

(b)  Build  the  roadmap  of  spine  net  M  =  (V,£),  where  V  is  the  collection  of 
all  the  directed  turns  (=  90°),  and  £  the  collection  of  all  directed  spines. 

Note  that  each  spine  has  two  directions,  6  and  6  +  180°,  where  0  =  0° 
or  90°.  Every  pair  of  intersecting  spines  generates  four  pairs  of  intersecting  directed 
spines  and  eight  distinct  directed  turns  at  each  junction  (see  Figure  3.7  for  an  illus¬ 
tration). 

4.  Data  Structure  of  the  Spine  Net 

Conceptually,  a  spine  net  consists  of  a  collection  of  vertices  which  are  di¬ 
rected  turns,  and  a  collection  of  edges  which  are  sections  of  directed  spines  connect¬ 
ing  two  directed  turns.  In  terms  of  data  structure,  a  spine  net  M  =  (V,£)  can  be 
constructed  and  represented  as  follows: 

1.  A  spine  is  determined  by  the  coordinates  of  its  two  end  points.  There  are  two 
directed  spines  associated  with  each  spine. 

2.  The  intersections  between  each  pair  of  spines  are  calculated.  If  there  are  0{m) 
spines,  there  will  be  0(m2)  intersections.  In  the  case  of  sensor-oriented  spine 
net,  m  =  0(n),  where  n  is  the  number  of  obstacle  walls  in  W .  In  the  case  of 
freeway  spine  net,  m  =  0(n2). 

3.  Each  pair  of  intersecting  spines  generates  eight  distinct  directed  turns  (ver¬ 
tices).  Therefore,  there  are  0(m2)  vertices. 
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Figure  3.8:  Construction  of  Spines  Using  Sensor-Oriented  Method 
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4.  Each  vertex  £  V  can  be  represented  as  an  exit  point  from  a  directed  spine  ex 
to  another  entry  point  on  another  intersecting  directed  spine  e 2.  In  the  case  of 
the  global  spine  net,  the  exit  point  is  coincident  with  the  entry  point. 

5.  For  every  two  vertices  tq,t;2  €  V,  if  the  entry  point  of  vx  lies  ahead  of  the 
exit  point  of  v2  on  the  same  directed  spine,  there  is  a  directed  edge  £  S  from 
vx  to  t>2.  The  directed  edge  is  a  segment  of  a  directed  spine,  and  is  implicitly 
represented  by  the  exit  point  and  the  entry  point.  There  are  potentially  0{m4) 
edges. 

5.  Time  Complexity  Analysis  of  Building  the  Spine  Net 

1.  Using  the  Freeway  Method 

(a)  Extracting  the  Freeway  Spines.  Assuming  there  are  n  obstacle  edges  in 
W .  The  number  of  potential  bisectors  is  0(n2).  Therefore,  extracting 
the  bisectors  takes  0(n2)  time.  Each  bisector  requires  0(n)  time  to  check 
against  intersection  with  obstacles  and  minimum  clearance  to  become  a 
freeway  spine.  Therefore,  extracting  the  freeway  spines  takes  a  total  of 
0(n3)  time. 

(b)  Building  the  Spine  Net.  Assuming  that  there  are  m  freeway  spines  ex¬ 
tracted.  Theoretically,  m  =  0(n 2),  although  m  is  shown  to  be  much  less 
in  number  from  empirical  results.  It  takes  0(m2)  time  to  compute  all  di¬ 
rected  turns  (the  vertices  of  Ad)  from  the  m  freeway  spines.  Finding  the 
edges  takes  0(m4)  time.  Therefore,  building  the  spine  net  takes  0{m4) 
time. 
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2.  Using  the  Sensor-Oriented  Method 


(a)  Extracting  the  Spines.  Assuming  there  are  n  obstacle  edges  in  W .  The 
number  of  potential  spines  is  0(n).  Therefore,  extracting  the  spines  takes 
0(n )  time.  Each  extended  spines  requires  0(n)  time  to  check  against 
intersection  with  obstacles.  Therefore,  extracting  the  spines  takes  a  total 
of  0(n2)  time. 

(b)  Building  the  Spine  Net.  Assuming  that  there  are  m  spines  extracted, 
where  m  =  0(n).  Using  the  same  reasoning,  building  the  spine  net  takes 
0(m4)  time. 

Conclusion  Building  the  spine  net  in  the  global  path  planning  layer  takes 
0(m4)  time,  where  m  =  0(n2)  using  the  freeway  method,  or  m  =  O(n)  using  the 
sensor-oriented  method. 
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IV.  CUBIC  SPIRALS 


A.  OVERVIEW 

The  cubic  spiral  is  first  investigated  in  [Ref.  12]  by  Y.  Kanayama  as  a  class  of 
smoothest  curves  with  respect  to  the  cost  function  of  ( dn/ds )2.  Only  the  section  of  a 
cubic  spiral  where  curvature  is  null  at  both  ends  are  used  in  motion  planning.  When 
such  sections  are  connected  to  form  a  motion  path,  a  path  that  is  of  class  C 2  in  its 
entirety  can  be  obtained.  Note  that  straight  line  segments  are  just  special  cases  of 
cubic  spirals.  This  makes  cubic  spirals  a  very  useful  tool  in  the  local  smooth  path 
planning  layer  to  smooth  out  all  the  turns  in  global  roadmaps  into  class  C2. 

In  contrast,  although  straight  line  segments  are  also  special  cases  of  circular 
arcs,  a  path  consisting  of  straight  line  segments  and  circular  arcs  is  not  C 2  in  its 
entirety.  This  is  because  while  straight  line  segments  have  null  curvature  at  two  ends, 
circular  arcs  have  constant  non-null  curvatures.  There  is  always  a  discontinuity  in 
curvature  when  a  straight  line  segment  meets  a  circular  arc. 

There  are  other  classes  of  curves  that  can  be  used  in  place  of  cubic  spirals  for 
planning  smooth  paths  of  class  C2.  Examples  are  the  clothoid  curve  [Ref.  14]  and 
the  B-spline  curve  [Ref.  15].  The  Cubic  spiral  is  chosen  in  this  thesis  because  of  its 
smoothness,  elegance,  and  ease  of  use  in  local  smooth  path  planning.  This  chapter 
introduces  the  properties  of  the  cubic  spiral  before  it  is  employed  in  the  subsequent 
chapters  of  motion  planning  layers. 

Some  readers  may  find  the  Mathematical  deductions  in  this  chapter  rather  ob¬ 
scure,  and  may  think  otherwise  about  the  elegance  of  cubic  spirals.  It  must  be 
pointed  out  that  the  basic  properties  of  the  unconstrained  cubic  spiral  are  not  dif¬ 
ficult.  Additional  Mathematical  deductions  are  required  in  this  chapter  because  of 
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the  kinematic  constraints  imposed,  and  the  inherent  difficulty  of  smoothing  out  the 
turns  of  global  roadmaps.  Whatever  curves  one  chooses  for  local  smooth  path  plan¬ 
ning  purposes,  the  Mathematics  required  will  always  be  much  more  than  the  basic 
properties  of  the  chosen  curve. 
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B.  BASIC  PROPERTIES  OF  CUBIC  SPIRALS 


While  most  people  are  familiar  with  the  geometry  of  the  circular  arcs  in  the 
near-smooth  path,  they  are  not  familiar  with  cubic  spirals  adopted  in  this  thesis  for 
smooth  path  planning.  The  readers  are  referred  to  [Ref.  12]  for  a  good  description  of 
cubic  spirals.  This  section  only  provides  a  quick  summary  of  the  important  properties 
of  cubic  spirals  related  to  path  planning: 

1.  A  cubic  spiral  with  length  /  and  maximum  curvature  Kj<a  (see  Figure  4.1)  is 
described  by  the  following  curvature  function: 

„  „  s(l  —  s)  ,  „ 

tt(-s)  =  Kdia  0  <  s  <  l  (4.1) 


2.  Cubic  spirals  have  no  closed  forms  in  Cartesian  coordinates  (see  Figure  4.2). 
A  point  (x,y)  =  (x(s),y(s))  on  a  cubic  spiral  is  given  by, 


x  =  x(s)  =  /  cos  6(t)dt 
Jo 

y  =  y(s)  =  /  sin  6{t)dt 

Jo 

Where  the  tangent  direction  function  9  is  given  by, 


x(s)  =  /  k (t)dt  0  <  s  <  1 

Jo 

Only  the  part  of  a  cubic  spiral  between  the  two  points  Si  and  s2  where  curvature 
is  null  will  be  used  in  path  planning. 


3.  Let  pi  =  (ii,yi,#i)  and  p2  =  (x2)2/2>^2)  be  two  configurations  with  (xi,yj)  ^ 
(^IjI/i)-  The  pair  of  two  configurations  pi  and  p2  are  said  to  be  symmetric  (see 
Figure  4.3  for  an  illustration)  if  and  only  if, 


tan 


y*  ~  y  i 
x2  -  Xi 


(4.2) 
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(Curvature  vs  Length) 


Figure  4.1:  Curvature  of  Cubic  Spiral 


Figure  4.2:  A  Cubic  Spiral  Curve  in  Cartesian  Coordinates 
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Figure  4.3:  Symmetric  Configurations 
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4.  A  cubic  spiral  is  uniquely  determined  by  a  pair  of  symmetric  configurations. 
In  particular,  it  is  uniquely  determined  by  the  size  d  and  the  deflection  a  of 
the  symmetric  pair  which  are  defined  as  the  Euclidean  distance  of  the  two 
points  and  the  angle  between  the  orientations  (see  Figure  4.4).  In  general,  it 
is  uniquely  determined  by  any  two  of  the  following  parameters: 

size  d,  deflection  a,  length  /,  height  h,  max  curvature  Kjt0r 


5.  If  two  symmetric  configurations  are  joined  by  two  straight  line  segments  (which 
are  special  cases  of  cubic  spirals)  and  a  cubic  spiral  (see  Figure  4.5),  then 
the  whole  path  is  of  class  C2.  In  other  words,  there  is  curvature  continuity 
throughout  the  whole  path  (except  at  the  two  path  ends). 


6.  All  cubic  spirals  with  the  same  deflection  a  are  similar  to  each  other  (see 
Figure  4.6).  That  is,  for  every  two  cubic  spirals  with  the  same  a  and  with 
parameters  li,dt,  hi,  /2,  d2,  h2  respectively,  the  following  relationship  holds: 

h  _  _  h\ 

I2  <^2  hi 

If  li  is  chosen  to  be  1  unit,  and  the  corresponding  di  and  hi  at  deflection  a  are 
denoted  to  be  D(a)  and  H(a)  respectively,  then, 


/  = 


(4.3) 


D{a)  H(a) 

Though  without  a  closed  form,  D(a)  can  be  obtained  from  the  following  inte¬ 
gral  equation, 

D(a)  =  2  J  cos  —  2f2j  t j  dt  (4.4) 


If  a  table  is  built  for  D(a)  for  0°  <  a  <  360°  (see  Figure  4.7),  the  length  l  of 
a  cubic  spiral  can  be  uniquely  determined  by  d  and  a  in  discrete  steps  of  the 
latter.  Likewise  applies  to  H(a). 
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Figure  4 


7.  As  seen  from  Figure  4.7,  when  a  increases,  D(a)  decreases.  D(a)  equals  to  0 
when  a  is  at  about  281°.  Since  cubic  spirals  with  D(a)  less  than  or  equal  to  0 
are  difficult  to  handle,  in  this  thesis,  only  those  cubic  spirals  with  deflection  a 
less  than  or  equal  to  280°  will  be  considered  for  path  planning  purposes. 


8.  The  maximum  curvature  occurred  at  a  cubic  spiral  determined  by  d  and  a 
is  denoted  as  Kja  (note:  this  is  different  from  the  kinematic  constraint  Kmax 
imposed  by  the  robot).  Because  of  the  symmetry  of  the  cubic  spiral,  it  occurs  at 
the  mid  point  of  the  curve  where  s  =  1/2  (see  Figure  4.1).  From  Equation  4.1, 
we  have 


Together  with  Equation  4.3,  we  have 


2k  d,gl 
3 


3a  3  aD(a) 

^  =  Ti  =  IT 


(4.5) 


or 


.  3  aD(a) 

a  =  — - - 


(4.6) 


9.  The  maximum  rate  of  change  of  curvature  occurred  at  a  cubic  spiral  deter¬ 
mined  by  d  and  a  is  denoted  as  k^Q  (note:  this  is  different  from  the  kinematic 
constraint  kmax  imposed  by  the  robot).  It  occurs  at  the  beginning  and  the 
end  of  the  curve  where  s  =  0  and  s  =  l  respectively  (see  Figure  4.1).  From 
Equation  4.1  ,  4.3  and  4.5,  we  have 


*d,a 


dn 

ds 


4Kd,o 


*=0 


6a 


6  qD2(q) 

I2 


(4.7) 
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C.  CIRCUMSCRIBING  CUBIC  SPIRALS  AND  INSCRIB¬ 
ING  CIRCLES 

This  section  studies  further  properties  of  cubic  spirals. 

Notation  An  angle  a  in  this  chapter  is  assigned  the  unit  degree  (°)  or  radian 
depending  on  the  context  it  is  in.  The  unit  degree  is  assumed  when  a  is  used  in  the 
context  of  plotting  curves  in  steps  of  1°,  or  is  subject  to  a  range  restriction.  This 
is  because  there  are  angles  of  special  interests,  for  example  280°,  which  can  not  be 
conveniently  expressed  in  radians.  On  the  other  hand,  the  unit  radian  is  assumed 
when  a  appears  in  Mathematical  formulas.  The  readers  should  have  no  problem  in 
making  the  distinction  for  a  given  context. 

Proposition  4.1  Given  a  cubic  spiral  uniquely  determined  by  a  pair  of  symmet¬ 
ric  configurations  px  and  p2  with  size  d  (>  0)  and  deflection  a  (<  280° ),  we  can 
uniquely  determine  a  circle  with  radius  r^Q  that  inscribes  the  cubic  spiral  at  the  two 
configurations  (see  Figure  f.8).  Inversely,  we  say  that  the  cubic  spiral  circumscribes 
the  circle  at  these  two  configurations.  We  call  them  the  “inscribing  circle”  and  the 
“circumscribing  cubic  spiral”  respectively.  From  geometry  of  the  circle,  we  have, 

_  d 

rd,a  2sin(a/2) 

Together  with  Equation  f.6,  we  have, 


Proposition  4.2  Any  two  points  on  a  circle  with  orientations  tangent  to  the  circle 
and  equal  in  direction  are  a  symmetric  pair.  Therefore,  they  can  be  circumscribed  by 
a  cubic  spiral  (see  Figure  f.8).  Let  r  be  the  radius  of  the  circle,  d(>  0)  and  a(<  280°) 
be  the  size  and  deflection  of  the  symmetric  configurations.  The  maximum  curvature 
incurred  at  the  circumscribing  cubic  spiral,  denoted  by  Kjia  can  be  determined  by  any 
two  of  the  above  three  parameters  (r,  d  and  a).  See  Equation  f.5  for  that  determined 
by  d  and  a,  and  Equation  4-10  for  that  determined  by  r  and  a. 


(4.8) 
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Figure  4.8:  A  Circle  Inscribing  a  Cubic  Spiral 


Proposition  4.3  Denote  by  l  the  length  of  the  cubic  spiral  with  size  d  and  deflection 
a  circumscribing  the  circle  of  radius  r.  From  Equation  f.5  and  f.9,  we  have, 

3a  6arsin(a/2)  n  /sin(a/2)\ 

~  ~  6D(a)(a/2)  =  2r  {  D(a)  ) 

Let 

e*  /  _  \  sin(a/2) 
f,(Q)  = 

Figure  f.9  shows  Ffla)  plotted  against  a  in  discrete  steps  of  1°.  It  shows  that, 

as  a  — ►  281°,  Fi(a)  — ►  oo,  therefore  /  — +  oo 

This  further  justifies  the  restriction  to  consider  only  cubic  spirals  of  deflection  a  < 
280°.  In  fact,  for  all  practical  purposes,  only  cubic  spirals  of  a  280°  need  to  be 
considered.  Proposition  IV. 11  in  section  E.  shows  a  method  of  avoiding  cubic  spirals 
with  large  deflections. 
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D.  CUBIC  SPIRALS  SUBJECT  TO  nmax  AND  kmax  CON¬ 
STRAINTS 

The  previous  sections  illustrate  the  general  properties  of  the  cubic  spiral,  and  the 
maximum  Kd<a  and  kd<a  incurred  at  each  cubic  spiral.  In  this  section,  the  properties 
of  the  subset  of  cubic  spirals  which  meets  the  gloabl  constraints  of  Kmax  and  kmax 
imposed  by  the  disc  robot  will  be  studied  in  detail. 

Proposition  4.4  Given  two  intersecting  lines  t\  and  e?  with  deflection  a  measured 
from  e\  to  ti,  and  0°  <  a  <  280°.  Ignoring  nmax  and  kmax  constraints,  we  can  obtain 
a  family  of  similar  cubic  spirals  with  deflection  a  connecting  any  pair  of  symmetric 
configurations  lying  on  e\  and  62  respectively  (see  Figure  4-6). 

Question  4.1  If  the  cubic  spiral  is  subject  to  Kmax  and  kmax  constraints,  find  the 
subset  of  this  a-deflection  family  of  cubic  spirals  that  can  meet  the  constraints. 


For  a  cubic  spiral  of  size  d  in  this  a-deflection  family,  denote  its  maximum 
curvature  and  maximum  rate  of  change  of  curvature  by  Kda  and  kda  respectively.  If 
it  meets  the  curvature  constraints,  we  must  have, 


<  Kmax  and  kd<a  <  kmax 


From  Equation  4.5,  we  have 


3  aD(a) 
2d 


—  Km0I 


or 


From  Equation  4.7,  we  have 


d  > 


3  aD(a) 

2k 

max 


Kd,a  — 


6  aD2{a) 
cP 


<  k 


_  "'max 


or 


d  >  D(a ) 


(411) 


(4.12) 
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From  Equation  4.11  and  4.12,  we  have, 


d  >  max 


/  3  aD(a) 


\ 


2kt 


(4.13) 


Proposition  4.5  The  subset  of  the  a-deflection  cubic  spiral  family  that  meets  the 
nmax  and  kmax  constraints  are  those  with  size  d  such  that  d  meets  the  condition  in 
Equation  4.13. 


Definition  4.1  If  a  circle  meets  the  condition  that  “every  two  symmetric  configura¬ 
tions  on  its  circumference  with  deflection  a  (<  280°)  can  be  circumscribed  by  a  cubic 
spiral  that  meets  the  nmax  constraint”,  we  say  that  the  circle  is  fully  Kmax  -admissible. 
Likewise  for  a  circle  that  is  fully  kmax-admissible. 


Question  4.2  Find  the  set  of  circles  that  are  fully  Kmax  -admissible. 


Suppose  we  have  a  circle  of  radius  r.  Every  two  symmetric  configurations  on  the 
circumference  of  the  cirle  with  deflection  a  can  always  be  circumscribed  by  a  cubic 
spiral  without  curvature  constraints.  Denote  the  maximum  curvature  and  maxi¬ 
mum  rate  of  change  of  curvature  of  the  circumscribing  cubic  spiral  by  Kdt0l  and 
respectively. 

If  the  circle  is  /cmai.-admissible,  then  we  must  have, 


^ d.a 


<  K 


max 


Combining  with  Equation  4.9,  we  have, 

r  >  3 DM  (  a/2  \ 

\sin(a/2)/ 


(4.14) 


Let 


0°<cr<280* 


/3P(o)[  a/2  |\=  3  ^  (D(a)(a/2)\ 

V2«mox  sin(a/2)  /  2Kmar  0"<o<280«  sin(a/2)  j 


and  let 


F2(a) 


D(o)H  2) 

sin(a/2) 
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We  can  easily  see  that  the  upper  bound  of  F2(a)  is  1, 


max  Fo 

0°  <o<280* 


(a)  =  (  max  D(a)]  (  max  -  =1x1  = 

\0*<q<280*  V  sin(a/2)  J 


Figure  4.10  shows  /^(a)  plotted  against  a  in  discrete  steps  of  1°.  It  confirms  that, 


max 

0°<a<280° 


Flic*) 


lim  F2(q)  =  1 

Q-.0*  v  ' 


Therefore, 


^  min  — 


2k. 


(4.15) 


Proposition  4.6  All  circles  with  radius  r  >  rmin  are  fully  Kmax- admissible. 
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Figure  4.10:  F2(a)  Plotted  Against  a  in  Steps  of  1° 
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Question  4.3  Find  the  set  of  circles  that  are  fully  kmax  -admissible. 


If  the  circle  is  «mai-admissible,  then  we  must  have, 


hd,a  <  « 


max 


From  Equations  4.7  and  4.6,  we  have, 


Kd,a 


6  q£>2(q)  . 

4r2  sin2(a/2)  — 


or 


r  > 


V  2/cmox 

Let 

r'  ;  =  max  ( 
o®<a<28o°  y 

/  3 

y/aD(a) 

V  max 

sin(a/2) 

and  let 

*»(«)  = 

y/aD(a) 


sin(o/2) 


(4.16) 


)=,0-(  max 
)  V  2/cmoI  \0»<a<280»  [sm(a/2)  / 


sin(or/2) 

Figure  4.11  shows  Fa(a)  plotted  against  a  in  discrete  steps  of  1°.  It  shows  that, 

as  a  — ♦  0,  /^(a)  —*  oo,  therefore  r'min  is  unbounded 
Proposition  4.7  No  circles  is  fully  kmax-admissible. 


Definition  4.2  Given  an  angle  0  where  0°  <  0  <  280°.  If  a  circle  meets  the  con¬ 
dition  that  “every  two  symmetric  configurations  on  its  circumference  with  deflection 
a,  where  0  <  a  <  280°,  can  be  circumscribed  by  a  cubic  spiral  that  meets  the  kmax 
constraint,  but  not  those  with  deflection  a  <  0”,  we  say  that  the  circle  is  partially 
kmax -admissible  at  0. 

Question  4.4  For  the  circle  with  radius  rmin  that  is  fully  Kmax -admissible,  find  0 
which  makes  it  partially  kmax -admissible  at  0. 
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From  Equation  4.16,  we  can  see  that  /?  occurs  at  the  point  where, 


1"min 


xfvwiL  nzFia) 

2k„„  \sin(l3/2))  \  2km„  3 


Together  with  Equation  4.15,  we  have 


—W) 

max 


or 


(4.17) 


There  is  no  closed  form  solution  for  /?;  we  can  only  find  an  approximate  value  for  /? 


from  the  curve  of  F3(a)  as  shown  in  Figure  4.11. 


Proposition  4.8  The  circle  with  radius  rmtn  that  is  fully  Kmax -admissible  is  par¬ 
tially  kmax -admissible  at  /3  as  determined  in  Equation  4- 17- 
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E.  BOUNDING  REGIONS  OF  CUBIC  SPIRALS 


Since  there  is  no  closed  form  for  cubic  spirals,  it  is  not  possible  to  check  for 
collisions  between  cubic  spirals  and  the  polygonal  obstacles  in  W  using  algebraic 
equations.  This  section  attempts  to  approximate  a  cubic  spiral  by  a  bounding  region 
such  that  the  cubic  spiral  lies  completely  within  the  bounding  region.  The  edges 
of  the  bounding  region  will  consist  of  only  straight  line  segments  and  circular  arcs. 
Instead  of  checking  the  cubic  spiral  against  the  obstacles,  we  will  check  the  bounding 
region  instead  which  allows  algebraic  solutions.  The  smaller  the  bounding  region, 
the  closer  the  approximation  and  hence  the  more  optimal  the  resulting  solution. 

Proposition  4.9  Given  a  cubic  spiral  uniquely  determined  by  a  pair  of  symmetric 
configurations  pi  and  pj  with  size  d  (>  0)  and  deflection  a  (<  180°j,  we  can  construct 
a  bounding  region  BR\  for  it  from  the  two  lines  generated  by  p\  and  p2  and  the 
circular  arc  that  inscribes  the  cubic  spiral  at  the  two  points  (see  Figure  f.12).  The 
radius  rja  of  the  inscribing  circle  is  given  by  Equation  f.8. 


Figure  4.12:  Bounding  Region  BRi  of  a  Cubic  Spiral 

As  a  increases,  so  does  the  area  of  BR\  of  the  cubic  spiral.  When  o  >  180°, 
BR\  is  undefined.  We  need  an  alternative  way  of  defining  the  bounding  region  when 
a  is  large. 
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Proposition  4.10  Given  a  cubic  spiral  uniquely  determined  by  a  pair  of  symmetric 
configurations  p\  and  p2  with  size  d  (>  0 )  and  deflection  a  (<  280° ),  we  can  construct 
a  bounding  region  BR2  for  it  from  the  two  lines  generated  by  pi  and  p 2,  its  inscribing 
circular  arc,  and  the  tangential  line  at  the  middle  of  the  curve  at  a  height  h  from  the 
base  ( see  Figure  f.13).  The  height  h  can  be  obtained  from  Equation  f.3  as, 


Figure  4.13:  Bounding  Region  BR2  of  a  Cubic  Spiral 

As  seen  from  Figure  4.7,  when  a  goes  beyond  281°,  D{ a)  becomes  negative.  The 
two  legs  of  the  cubic  spiral  cross  and  BR2  is  ill-defined.  This  justifies  the  restriction 
to  consider  cubic  spirals  of  deflection  a  <  280°.  In  the  next  chapter,  it  will  be 
shown  that  this  restriction  is  sufficient  for  local  smooth  path  planning.1  Hence  the 
bounding  region  BR2  is  sufficient  for  use  in  most  cases. 

As  o  increases,  so  does  the  area  of  BR2  of  the  cubic  spiral.  Although  the  area  of 
BR2  is  always  smaller  than  that  of  BR\,  and  is  thus  a  better  approximation  for  the 
cubic  spiral,  it  can  still  be  excessive  for  large  a.  The  following  proposition  develops 
a  technique  to  get  around  this  problem.  It  also  gets  around  the  problem  of  making 
a  turn  of  deflection  a  >  280°  using  cubic  spirals. 


’This  is  not  true  for  the  end  portions  path  planning,  which  requires  breaking  a  cubic  spiral  into 
a  sequence  of  sub  cubic  spirals. 
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Figure  4.14:  Substituting  a  Cubic  Spiral  with  a  Sequence  of  Sub-Spirals 


Proposition  4.11  Given  two  configurations  px  and  p?  on  a  Kmax -admissible  circle 
with  size  d  (>  0)  and  deflection  a  (<  360° ),  we  can  always  join  them  by  a  sequence 
of  n  cubic  spirals,  each  of  deflection  atj  —  a/n  and  satisfying  the  Kmax  constraint,  and 
the  sequence  is  of  class  C 2  in  its  entirety.  The  bounding  region  of  this  sequence  of  n 
cubic  spirals  is  the  union  of  the  bounding  regions  of  each  sub  cubic  spiral,  which  is 
smaller  in  area  than  the  bounding  region  of  the  original  cubic  spiral  (see  Figure  4-1 4). 

Proof  Simply  divide  the  circular  arc  between  p\  and  p2  into  n  —  1  equal  parts 
separated  by  qi  •  •  •  qn-2-  Connect  Pi >  9i 92 » '  *  •  ,gn-2P2  by  cubic  spirals.  It  can  be 
easily  shown  that  each  cubic  spiral  is  of  deflection  a,  =  a/n.  Since  the  inscribing 
circle  is  Kmai-admissible,  all  the  n  sub  cubic  spirals  meet  the  nmax  constraint.  In 
general,  a,  need  not  be  of  equal  size.  They  need  only  be  small  enough,  say  <  90°, 
to  produce  a  convenient  sequence  of  sub-spirals. 


V.  LOCAL  SMOOTH  PATH  PLANNING 

LAYER 


A.  OVERVIEW 

The  local  smooth  path  planning  layer  is  the  second  layer  in  the  layered  motion 
planning  approach  proposed  in  this  thesis.  The  goal  of  this  layer  is  to  modify  the 
coarse  global  roadmap  obtained  from  the  first  layer  into  a  smooth  roadmap.  A  smooth 
roadmap  is  one  where  every  path  in  it  is  smooth.  This  layer  is  also  a  preprocessing 
step  that  needs  to  be  carried  out  only  once  together  with  the  global  path  planning 
layer  to  serve  all  subsequent  on-line  queries  of  motion  planning. 

Various  kinds  of  curves  could  be  used  for  smoothing  purposes.  This  thesis 
chooses  a  class  of  curves  called  cubic  spirals.  Since  straight  line  segments  are  just 
special  cases  of  cubic  spirals,  a  smooth  roadmap  is  said  to  consist  of  only  cubic 
spirals. 

Regardless  of  which  kind  of  smooth  curve  is  chosen,  this  layer  (and  the  end 
portions  path  planning  layer  to  be  discussed  in  the  next  chapter)  faces  the  same 
challenges  as  follows: 

1.  Tne  smooth  curve  that  is  adopted  for  a  turn,  or  is  replacing  an  original  circular 
turn,  must  be  collision-free.  There  are  two  sub-tasks  involved  here:  one  is  to 
fii  d  plausible  candidates  in  polynomial  time,  another  is  to  check  them  against 
obstacles.  So  far,  none  of  the  smooth  curves  that  have  nice  properties  in 
curvatures  has  closed  form  in  Cartesian  coordinates.  If  we  fixed  the  number 
of  incremental  steps,  tracing  out  an  approximate  trajectory  of  the  smooth 
curve  can  be  achieved  in  polynomial  time.  However,  this  only  helps  in  collision 
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checking,  it  does  not  facilitate  candidates  generation.  This  thesis  proposes  the 
bounding  region  concept  described  in  Chapter  IV  to  tackle  this  problem. 

2.  The  smooth  curve  is  subject  to  two  kinematic  constraints:  Kmax  and  kmaz.  This 
severely  reduces  the  number  of  candidates  and  the  chances  of  completing  a  turn 
with  smooth  curves.  This  problem  is  most  evident  in  the  case  of  visibility  net 
where  the  concern  must  be  brought  up  during  the  global  path  planning  layer 
to  increase  the  chances  of  success.  Modifications  required  for  the  construction 
of  the  visibility  net  will  be  discussed  in  detail  in  this  chapter.  In  addition, 
additional  Mathematics  are  required  to  study  the  properties  of  the  subset  of 
the  smooth  curves  which  meet  the  kinematic  constraints.  Such  Mathematics 
are  normally  tedious  and  non-trivial  as  can  be  seen  in  Chapter  IV. 

Although  only  cubic  spirals  are  used  in  this  thesis,  different  techniques  are 
required  for  smoothing  different  classes  of  roadmaps.  In  this  thesis,  roadmaps  are 
classified  according  to  their  geometric  properties,  not  the  methods  from  which  they 
are  generated.  Roadmaps  in  the  same  class  will  have  similar  basic  data  structure 
representation.  At  present,  three  kinds  of  roadmaps  have  been  classified  in  the  global 
path  planning  layer,  namely,  the  visibility  net,  the  nominal  configuration  net  and 
the  spine  net. 

In  the  following  sections,  the  smoothing  techniques  for  each  class  of  roadmaps 
will  be  investigated  and  their  time  complexities  analyzed.  In  some  cases,  modifica¬ 
tions  to  the  data  structure  are  required  for  the  smooth  roadmap. 
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B.  SMOOTHING  TECHNIQUE  FOR  THE  VISIBILITY 
NET 

1.  Shortcomings  of  the  Unconstrained  Visibility  Net 

At  a  glance,  smoothing  a  visibility  net  seems  trivial:  just  replace  each 
circular  turn  by  a  cubic  spiral  turn,  and  then  check  for  collision.  This  is  true  if  there 
is  no  concern  of  the  success  rate  of  this  operation.  If  the  visibility  net  is  built  in  the 
global  path  planning  layer  without  considering  the  kinematic  constraints,  many  of 
the  circular  turns  may  not  be  replaceable  by  constrained  cubic  spirals.  In  fact,  if  it 
is  built  with  r0  <C  rmin,  none  of  the  circular  turns  may  be  smoothed  out. 

Such  complications  of  the  visibility  net  are  due  to  the  peculiarity  of  the 
visibility  graph  method,  where  the  turns  are  planned  as  close  to  the  corners  as 
possible.  As  a  result,  the  constraints  of  the  smooth  path  must  be  considered  at  the 
global  path  planning  layer.  Otherwise,  the  unconstrained  visibility  net  may  be  too 
“tight”  to  allow  for  local  smooth  path  planning.  This  problem  is  not  faced  by  the 
spine  net,  where  the  turns  are  planned  at  moderate  distance  (not  as  far  as  possible) 
from  a  turning  corner.  There  are  “normally”  enough  allowance  between  the  turns 
and  the  corners  for  local  smooth  path  planning  in  the  spine  net. 

In  the  following  discussions,  the  visibility  net  will  be  rebuilt  subject  to 
the  Kmax  constraint  first,  and  then  the  kmax  constraint  next.  Thereafter,  a  not-so- 
straight-forward  smoothing  technique  will  be  proposed  to  increase  the  success  rate 
of  this  layer. 

2.  Rebuilding  the  Visibility  Graph  Subject  to  Kmax  Con¬ 
straint 

In  the  unconstrained  visibility  graph  M,  the  visible-directed-circular-arcs 
(from  now  on  abbreviated  as  circular  arcs)  are  of  radius  r0,  which  is  the  radius  of  V. 
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From  proposition  IV. 6  and  equation  4.15,  if 

.  3 

r0  _  7*min  —  n 

max 

then  all  circular  arcs  can  be  circumscribed  by  cubic  spirals  satisfying  the  Kmaz  con¬ 
straint.  However,  if  r0  rmtn,  then  most,  if  not  all,  of  the  circular  arcs  will  not  be 
replaceable  by  constrained  cubic  spirals. 

A  first-cut  solution  seems  easy;  just  grow  the  corners  of  the  obstacles  in  W 
by  rm,„  instead  of  r0.  Unfortunately,  there  are  three  problems  with  this  solution: 

1.  Where  V  can  pass  by  a  corner  with  clearance  r0,  the  new  bumper  of  rmm  >  r0 
forces  T>  to  make  an  unnecessary  turn  (see  Figure  5.1(a)). 

2.  When  V  makes  a  turn  about  a  corner,  it  needs  only  maintain  a  clearance  of 
r0  at  its  closest  point  to  the  corner  (see  Figure  5.1(b)).  Growing  the  corner  by 
rmin  is  too  excessive. 

3.  By  growing  the  corners  by  rmjn  >  r0,  some  of  the  channels  may  become  in¬ 
accessible  (see  Figure  5.1(c)),  while  in  fact  V  can  pass  through  them  without 
difficulty  (see  Figure  5.1(d)). 

A  better  solution  is  to  construct  M  =  (V,£)  with  the  following  steps: 

Procedure  Build  Visibility  Net  2: 

1.  Denote  the  original  convolution  diagram  by  C.  Construct  another  convolution 
diagram  C'  by  circumscribing  each  circular  vertex  in  C  at  its  tip  with  a  circle 
of  radius  rmin  (see  Figure  5.2). 

2.  Construct  V  and  S  based  on  C  with  the  exception  that  visibility  of  an  edge  in 
€  is  checked  against  C,  not  C' .  This  will  ensure  that  a  disc  robot  T>  need  not 
make  an  unnecessary  turn  when  it  can  pass  by  a  corner  in  a  straight  line. 
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Figure  5.1:  Problems  of  Growing  Obstacle  Corners  by  rmin  >  r0 
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Figure  5.2:  Circumscribing  Circular  Vertex  with  Circle  of  radius  rmin 

Where  there  are  narrow  passages,  C1  may  still  show  that  it  is  not  possible 
to  construct  a  circular  vertex  (see  Figure  5.1(c))  while  in  fact  it  is  possible.  The 
trick  is  to  circumscribe  the  original  circular  vertex  at  a  point  that  can  circumvent 
the  constraints  of  the  environment  (see  Figure  5.1(d)).  However,  such  a  fine  point 
of  optimality  will  not  be  dealt  with  in  this  thesis. 

3.  Rebuilding  the  Visibility  Graph  Subject  to  kmax  Con¬ 
straint 

The  kmax  constraint  is  the  most  irritating  problem  in  constructing  the 
visibility  graph.  The  reason  is  simply  because  there  is  no  circles  that  is  fully  kmax- 
admissible  (see  proposition  IV. 7).  Even  if  r0  »  rmm,  this  problem  still  persists  in 
non-trivial  cases.  In  the  following  discussions,  Procedure  Build  Visibility  Net  2  will 
be  modified  to  cater  for  the  additional  kmax  constraint. 

Let  rmin  =  rnax(rmi„,ro).  According  to  proposition  IV. 8,  there  is  a  (3  where 
all  turns  of  angle  >  (3  can  also  satisfy  the  kmax  constraint.  Denote  the  size  of  the 
cubic  spiral  at  deflection  /3  as  dp,  and  the  chord  supporting  it  chordp.  The  remaining 
problem  is  to  handle  turns  with  deflection  a  <  (3  subject  to  the  kmax  constraint. 
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From  equation  4.12,  we  know  that  all  cubic  spirals  with  size 


da  >  D(a) 


can  meet  the  kmax  constraint.  Since  F+  =  \/aD(a)  is  a  non-decreasing  function  in 
the  range  0°  <  a  <  109°  (see  Figure  5.3),  it  follows  that 


dQ  <  dp  ,  where  0°  <  a  <  /?  <  109° 


That  is,  for  every  turn  of  angle  a  <  ft,  if  we  find  another  inscribing  circle 
with  chord  length  dp,  we  can  find  a  corresponding  cubic  spiral  that  can  meet  the 
kmax  constraint  (see  Figure  5.4(b)).  Also,  since  the  radius  of  this  circle  is  clearly 
greater  than  rmtn,  the  cubic  spiral  can  meet  the  Kmax  constraint  as  well. 

Unfortunately,  the  problem  has  not  been  fully  resolved  yet  at  this  point, 
which  makes  this  approach  very  sticky.  First,  the  arc  of  the  new  circle  which  is 
the  bounding  edge  of  the  cubic  spiral,  intersects  the  circle  with  radius  rmtn.  This 
means  that  the  bounding  region  will  collide  with  the  corners  of  the  C-obstacles.  One 
solution  is  to  grow  the  rm,n  circle  by  a  bumper  of  A r,  where  the  chordp  of  the  circle 
of  radius  rp  =  rmtn  +  A r  just  touches  the  rm,„  circle  (see  Figure  5.4(a)).  Note  that 
the  rp  circle  is  concentric  with  the  rmjn  circle,  not  tangential.  From  geometry,  we 
have 


T’min 

re  "  cos(/3/2) 


(5.1) 


or 


A r  =  rp-  rmin  =  rmin(l  -  sec(£/2)) 


(5.2) 


Second,  if  two  turns  of  angle  <  /3  are  very  close,  the  /3-chords  of  both  may 
intersect  each  other,  thus  no  solution  for  collision-free  cubic  spirals  for  them  (see 
Figure  5.5). 
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There  is  clearly  a  solution  for  them,  and  it  will  be  preposterous  of  this 
procedure  to  declare  no  solution  at  this  point.  One  not-very-elegant  way  to  get 
around  this  problem  is  to  create  a  third  rp  circle  further  down  the  road  to  make 
room  for  two  ^-chords  (see  Figure  5.6)  just  for  these  two  circular  vertices.  All 
vertices  lying  on  the  second  rp  circle  that  are  too  close  to  the  first  rp  circle  are 
removed  and  recomputed  on  the  third  rp  circle. 

With  these  additional  considerations  for  kmax  constraint,  the  visibility  net 
M.  can  be  rebuilt  using  Procedure  Build  Visibility  Net  2  with  all  the  necessary  mod¬ 
ifications. 

4.  The  Smoothing  Technique 

After  rebuilding  the  visibility  net  taking  into  accounts  the  kinematic  con¬ 
straints,  what  is  left  to  be  done  is  to  construct  collision-free  cubic  spirals  for  each 
turn  around  a  circular  vertex  in  the  C-space.  Recall  that  the  edges  in  M  are  made 
up  of  directed  tangent  edges  and  directed  circular  arcs,  and  the  vertices  are  made  up 
of  directed  tangent  contacts.  Two  directed  tangent  contacts  lying  on  the  same  circu¬ 
lar  vertex  with  the  same  orientations  (-(-  or  — )  determines  a  turn.  If  a  collision-fre^ 
cubic  spiral  can  be  constructed  for  a  turn,  it  replaces  the  corresponding  directed  cir¬ 
cular  arc  in  S  of  M..  If  it  cannot  be  found,  the  corresponding  directed  circular  arc  is 
removed  from  £,  and  the  two  vertices  are  disconnected.  The  resulting  smoothed-out 
visibility  net  M  is  called  a  smooth  M. 

Let  a  denote  the  angle  of  a  turn,  and  /?  be  the  partially  «mai-admissible 
angle  for  the  circle  with  radius  rp  =  rmin  -f  A r.  The  following  technique,  which  has 
largely  been  discussed  in  the  last  subsection,  can  be  used  to  find  a  collision-free  cubic 
spiral  for  each  turn: 

1.  If  a  >  /?,  BRi  or  BR2  for  the  cubic  spiral  can  be  constructed  directly  to  check 
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for  collisions.  If  the  bounding  region  collide  with  obstacles,  project  the  collision 
points  onto  the  inscribing  circle  (the  circular  vertex),  and  construct  a  sequence 
of  sub  cubic  spirals  based  on  these  constrained  points.  If  any  of  these  sub  cubic 
spirals  has  a  <  /?,  report  failure.  Otherwise  check  the  union  of  their  bounding 
regions  for  collision.  If  it  is  collision  free,  report  success.  Otherwise,  report 
failure. 

2.  If  a  <  /?,  find  another  inscribing  circle  with  chord  length  dp.  Follow  the 
procedures  in  last  step  to  check  for  collisions  against  the  bounding  region  of 
the  cubic  spiral. 

5.  Time  Complexity  Analysis 

Assuming  there  are  n  obstacle  edges  and  O(n)  obstacle  corners  in  W.  There 
are  all  together  0(n2)  vertices  in  the  visibility  net.  Checking  the  bounding  region 
of  each  vertex  aga:nst  collision  takes  O(n)  time.  Therefore,  it  takes  0(n3)  time  to 
smooth  out  the  visibility  net. 
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C.  SMOOTHING  TECHNIQUE  FOR  THE  NOMINAL  CON¬ 
FIGURATION  NET 

1.  The  Smoothing  Technique 

Recall  that  in  a  nominal  configuration  net  M,  the  vertices  are  the  nominal 
configurations,  and  the  edges  are  the  logical  connections.  The  smoothing  technique 
in  this  case  is  simply  to  replace  each  logical  edge  by  a  physical  cubic  spiral  connecting 
the  vertices,  followed  by  checking  the  cubic  spiral  against  the  kinematic  constraints 
and  possible  collision  with  the  obstacles.  If  the  checking  fails,  remove  the  edge  from 
M..  The  only  complication  of  this  technique  is  when  two  adjacent  vertices  are  not 
a  symmetric  pair.  Constructing  a  cubic  spiral  for  a  non-symmetric  pair  involves  the 
computation  of  a  split  configuration  in  between,  and  the  construction  of  two  sub  cubic 
spirals.  This  procedure  is  quite  straight  forward  for  a  parallel  non-symmetric  pair, 
but  is  non-trivial  for  a  non-parallel  non-symmetric  pair.  The  readers  are  referred  to 
[Ref.  12]  for  a  good  description  on  non-symmetric  pairs. 

2.  Time  Complexity  Analysis 

Assuming  there  are  n  obstacle  edges  in  W .  There  are  O(n)  vertices  and 
edges  in  the  nominal  configuration  net.  Checking  the  bounding  region  of  each  edge 
against  collision  takes  O(n)  time.  Therefore,  it  takes  0(n2)  time  to  smooth  out  the 
nominal  configuration  net. 

3.  Critique 

While  the  nominal  configuration  net  can  be  elegantly  smoothed  in  some 
cases  (see  Figure  5.7),  it  fails  in  some  other  carefully  chosen  examples  (see  Figure  5.8). 
The  nominal  configuration  net  method  is  a  very  good  example  of  an  intuitively 
appealing  method  which  quickly  reveals  its  limitation  under  scrutiny. 
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D.  SMOOTHING  TECHNIQUE  FOR  THE  SPINE  NET 


1.  Background 

The  essence  of  this  technique  is  to  construct  collision-free  cubic  spirals 
for  each  directed  turn  (vertex)  in  V  of  M-  Out  of  the  eight  directed  turns  at  each 
junction,  two  each  share  the  same  cubic  spiral  in  different  directions  (see  Figure  3.7). 
Therefore,  there  are  at  most  four  distinct  cubic  spirals  at  each  turn.  A  pair  of  directed 
turns  in  opposite  directions  is  simply  called  a  “turn”.  If  it  is  not  possible  to  construct 
a  collision-free  cubic  spiral  for  a  turn  under  the  kinematic  constraints,  remove  the 
corresponding  vertex  from  V.  As  mentioned  in  Chapter  IV,  there  is  no  closed  form 
of  cubic  spirals  for  geometric  reasoning.  Collisions  against  obstacles  will  be  checked 
using  their  bounding  regions. 

2.  The  Smoothing  Technique 

A  turn  (<  180°)  is  defined  by  a  pair  of  intersecting  directed  spines.  Let 
the  angle  of  the  turn  be  a.  A  constrained  and  collision-free  cubic  spiral  can  be 
constructed  for  each  turn  in  (or  to  determine  failure)  with  the  following  steps: 

1.  The  first  step  is  to  find  the  subt^t  of  the  a-deflection  cubic  spiral  family  that 
meets  the  Kmax  and  kmax  constraints.  From  proposition  IV. 5  and  equation  4.13, 
this  subset  of  cubic  spirals  are  those  with  size  d  such  that, 

6q  \ 

Kmax  / 

2.  The  second  step  is  to  find  the  corresponding  radii  of  the  inscribing  circles  of 
this  subset  of  cubic  spirals.  This  step  is  necessary  for  the  construction  of  the 
bounding  regions  (be  it  BR\  or  BRi)-  Let  r  be  the  radius  of  the  inscribing 


d  >  dmtn  =  max 


(3aD(a) 

h^r"D(Q)' 
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circle  of  the  bounding  region.  From  equation  4.13  and  equation  4.8,  we  have, 


r  >  rmi„  =  max 


3  otD(a) 


D(a) 


I  6a  \ 

^moi  / 


(5.3) 


,4 Kmax  sin(o/2)  ’  2  sin(o/2) 

Note  that  the  centers  of  the  inscribing  circles  always  lie  on  the  bisector  of  the 
spines  making  the  turn. 


3.  The  third  step  is  to  find  the  intervals  of  r  that  are  collision-free.  In  this  step, 
we  represent  the  disc  robot  V  as  a  point  robot  in  its  C-space.  A  radius  r  is 
said  to  be  critical  if  it  touches  some  C-obstacles.  If  there  are  m  obstacle  edges 
and  n  obstacle  vertices,  then  there  are  at  most  m  +  n  critical  radii.  A  critical 
interval  is  defined  as  the  interval  between  two  successive  critical  radii.  Wilfong 
[Ref.  7]  shows  that  all  radii  in  a  critical  interval  are  either  collision  free  or  all 
collide  with  some  obstacles. 

Note  that  a  turn  must  be  completed  within  the  two  intersecting  directed  spines. 
It  is  given  that  the  angle  of  the  turn  is  a(<  180°).  Let  the  two  intersecting 
directed  spines  be  ex  and  e 2.  Let  lgx  be  the  length  of  the  portion  of  ex  before 
the  turn,  and  lg2  the  length  of  the  portion  of  e2  after  the  turn.  Without 
loss  of  generality,  assume  lgx  <  lg? .  Let  rmaz  be  the  maximum  radius  that 
the  inscribing  circle  of  the  cubic  spiral  turn  can  assume.  From  geometry  (see 
Figure  5.9),  we  have, 

rm ax  =  I91  tan(90°  -  |)  (5.4) 

A  further  restriction  on  the  collision-free  intervals  of  r  is  thus, 


T'mtn 


<  r  <  r 


mar 


4.  The  fourth  step  is  to  check  BR\  against  collision.  Suppose  [rq ,  r2],  [r3,  r4], . . . ,  [rn_ j ,  rn] 
are  the  collision-free  critical  intervals  (see  Figure  5.10)  with 

rm<n  <  rx  <  r2  <  <  rn  <  rmaI 
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fr  r 

rn.t) 

Figure  5.9:  Computing  rmax  for  a  turn 


Check  only  the  BRi  bounded  by  the  circular  arc  of  radius  rj  and  the  two 
spines  t\  and  e2  for  collision.  If  BR\  is  collision-free,  return  success.  Otherwise, 
proceed  to  the  next  step.  Note  that  this  step  always  return  the  smallest  possible 
inscribing  circle  for  the  cubic  spiral  of  a  turn. 

5.  The  fifth  step  is  to  check  BR 2  against  collision.  For  each  collision-free  critical 
interval  [r*,  rfc+i],  construct  a  corresponding  BR 2  bounded  by  the  circular  arc  of 
radius  r*+J,  the  two  spines  e\  and  e2,  and  the  tangential  cap1  (see  Figure  5.11). 
Check  against  each  fl/?2  for  collision.  If  one  of  the  BR2  is  collision-free,  return 
success.  Otherwise,  proceed  to  the  next  step.  Note  that  this  step  always  return 
the  largest  r  of  the  smallest  possible  critical  interval  for  the  cubic  spiral  of  a 
turn. 

6.  The  last  step  is  to  find  a  sequence  of  sub  cubic  spirals  (see  proposition  IV.  11), 
each  with  deflection  a  >  /?,  which  circumscibed  each  critical  radius  r*.  Check 
the  union  of  their  bounding  regions  against  collision.  If  one  of  the  bounding 

'There  is  a  conjecture  that  the  two  circular  arcs  of  radius  r*  and  r*  +  |  and  the  two  spines  can 
form  a  bounding  region  BR3  for  a  cubic  spiral  if  ri  +  J  is  suitably  larger  than  rt.  If  this  can  be 
proved,  then  we  can.  ai  ways  check  BR3  (which  is  trivial)  before  B/?2. 
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region  union  is  collision-free,  return  success.  Otherwise,  return  failure.  Note 
that  this  step  offers  the  closest  approximations  for  the  cubic  spirals. 

3.  Time  Complexity  Analysis 

1.  Constructing  the  Convolution  Diagram.  Assuming  there  are  n  obstacle  edges 
in  W.  The  convolution  diagram  can  be  constructed  in  0(n2logn)  time  using 
simple  ray  sweep  technique,  or  in  O(nlogn)  time  using  Voronoi  diagram. 

2.  Smoothing  out  the  Spine  Net.  There  are  0(m2)  vertices  in  the  spine  net.  For 
each  vertex  (directed  turn),  there  are  O(n)  critical  radii  which  can  be  com¬ 
puted  in  O(n)  time.  Sorting  them  in  increasing  order  starting  at  the  smallest 
critical  radius  to  determine  the  collision-free  critical  intervals  takes  O(nlogn) 
time.  For  each  critical  interval  of  a  directed  turn,  checking  the  bounding  re¬ 
gion  against  the  C-obstacles  takes  0(n)  time.  Therefore,  it  takes  a  total  of 
0(m2n 2)  time  to  smooth  out  the  spine  net. 

Conclusion  Smoothing  out  the  spine  net  in  the  local  smooth  path  planning 
layer  takes  0(m2n2)  time,  where  m  =  0(n 2)  in  the  case  of  freeway  method,  or 
m  =  0(n)  in  the  case  of  sensor-oriented  method. 

4.  Modifications  to  the  Data  Structure 

In  this  layer,  two  modifications  are  made  to  the  spine  net  M: 

1.  If  a  collision-free  cubic  spiral  cannot  be  constructed  for  a  turn  under  the  kine¬ 
matic  constraints,  the  two  corresponding  directed  turns  (vertices)  are  removed 
from  V.  All  edges  incident  with  these  two  vertices  are  also  removed  from  S. 

2.  The  exit  point  and  the  entry  point  of  a  directed  turn  (vertex)  are  updated 
according  to  those  of  the  corresponding  cubic  spiral  turn. 
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VI.  END  PORTIONS  PATH  PLANNING 

LAYER 


A.  OVERVIEW 

The  end  portions  path  planning  layer  is  the  third  layer  in  the  layered  motion 
planning  approach  proposed  in  this  thesis.  Given  S  and  G,  the  goal  of  this  layer  is  to 
find  some  candidates  of  intermediate  So  (can  be  coincident  with  S)  and  intermediate 
G0  (can  be  coincident  with  G)  such  that: 

1.  It  is  possible  to  plan  a  feasible  path  from  S  to  So. 

2.  It  is  possible  to  merge  So  into  the  smooth  M.  using  only  cubic  spirals. 

3.  It  is  possible  to  diverge  from  the  smooth  M  into  G0  using  only  cubic  spirals. 

4.  It  is  possible  to  plan  a  feasible  path  from  Go  to  G. 

At  the  end  of  this  layer,  a  number  of  candidate  intermediate  configurations  of  So 
and  Go  would  have  been  found.  Given  W ,  this  layer  is  a  per  query  step  that  needs 
to  be  carried  out  for  each  on-line  queries  of  motion  planning.  A  query  is  a  request 
to  plan  a  smooth  path  for  an  initial  configuration  S  and  a  final  configuration  G  in 
free(W). 

Section  B.  of  this  chapter  defines  the  concepts  and  terms  used  in  this  chapter. 
Section  C.  considers  the  basic  problem  of  maneuvering  a  point  configuration  into 
a  directed  line  segment  using  circular  arcs  of  radius  Rmin  —  l/«mox  and  tangent 
line  segments.  Section  D.  considers  the  inverted  problem  of  maneuvering  from  a 
directed  line  segment  into  a  point  configuration.  Section  E.  modifies  the  basic  and 
the  inverted  problems  into  using  cubic  spirals  for  the  last  maneuver  subject  to  Kmax 
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and  kmax  constraints.  Section  F.  makes  use  of  the  techniques  developed  in  earlier 
sections  to  complete  the  end  portions  path  planning  problem  of  the  spine  net.  Lastly, 
section  G.  analyses  the  time  complexity  of  the  end  portions  path  planning  technique. 
Important  Notes: 

1.  The  end  portion  path  planning  technique  developed  in  this  chapter  is  targeted 
only  for  spine  nets.  It  can  be  applied  to  the  visibility  net,  but  the  result 
is  less  satisfactory.  This  is  because  the  technique  in  this  chapter  looks  for 
spines  for  merging,  while  in  the  case  of  visibility  net,  another  technique  which 
looks  for  circular  vertex  for  merging  would  be  more  optimal.  This  technique 
is  not  applicable  to  the  nominal  configuration  net  at  all.  Time  permitted,  the 
author  may  investigate  into  the  techniques  for  the  visibility  and  the  nominal 
configuration  nets  in  future. 

2.  In  this  chapter,  the  die  robot  V  is  always  represented  as  a  point  configuration 
p  in  its  configuration  space  (C-space). 
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B.  DEFINITIONS 


Given  a  directed  line  segment  e,  denote  the  directed  line  extended  from  e  as 
/(e).  A  circle  can  lie  in  three  positions  relative  to  /(e),  and  hence  to  e:  C  (left),  TZ 
(right),  and  O  (on).  See  Figure  6.1. 


on 


Figure  6.1:  Relative  Positions  of  Circles  Against  a  Directed  Line  Segment 


There  are  two  orientations  defined  for  a  circle:  L  mode  (CCW)  and  R  mode 
(CW).  Likewise  applies  to  a  circular  arc. 

Combining  relative  positions  and  orientations,  there  are  six  possible  relative  con¬ 
figurations  a  circle  can  have  against  a  directed  line  segment:  CL,  CR ,  1ZL,  7 ZR,  OL,  andOR , 
where  the  first  letter  represents  the  relative  position  and  the  second  letter  the  orien¬ 
tation.  See  Figure  6.2. 


Figure  6.2:  Relative  Configurations  of  Circles  Against  a  Directed  Line  Segment 
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A  circle  with  curvature  k^x  has  radius  l//cmor  denoted  by  We  call  the 

circle  an  /Lnm-circle.  Likewise  applies  to  an  7Lnm-&rc- 

Given  a  point  robot  p  (represented  by  its  configuration),  two  circles  tan¬ 
gential  to  p  can  be  defined:  one  is  an  L-circle,  the  other  an  /2-circle.  If  the  circles  are 
obstructed  by  obstacles,  L- arc  and  R- arc  are  defined  instead.  These  two  tangential 
arcs  can  be  further  divided  into  four  maneuvering  arcs:  forward  L- arc,  backward 
L-arc,  forward  /2-arc,  and  backward  R- arc.  The  concept  of  forward  and  backward  is 
based  on  the  maneuvering  direction  (forward  or  backing-up)  of  p  on  the  maneuvering 
arc.  See  Figure  6.3. 


Figure  6.3:  Four  Types  of  Maneuvering  Arcs  Tangential  to  p 

The  extremity  of  a  maneuvering  arc  is  where  p  maneuvers  to  the  other  end  of 
the  maneuvering  arc  and  hits  the  obstacle  wall.  From  the  extremity,  p  can  turn  the 
front  wheels  to  the  opposite  maximum  steering  angle  and  reverse  its  velocity,  thus 
tracing  out  a  new  maneuvering  arc  with  opposite  mode  and  opposite  maneuvering 
directions.  Note  that  two  consecutive  maneuvering  arcs  share  a  common  vertical 
tangent.  For  example,  from  the  extremity  of  a  forward  L-arc,  a  backward  /2-arc  can 
be  generated.  See  Figure  6.4. 

The  number  of  maneuvers  of  a  path  is  defined  as  the  number  of  maneuvering 
arcs  involved  in  the  path.  Intuitively,  the  less  number  of  maneuvers,  the  better  the 
solution  path. 
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Figure  6.4:  Tracing  Out  a  New  Maneuvering  Arc  from  an  Extremity 

When  p  is  maneuvered  from  an  initial  configuration  S  into  a  directed  line  seg¬ 
ment  e,  p  (or  5)  is  said  to  merge  into  e.  When  p  is  maneuvered  from  e  into  a  final 
configuration  G,  p  (or  (?)  is  said  to  diverge  out  of  e.  These  terminologies  will  be 
used  in  subsequent  discussions. 
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C.  MERGING  A  CONFIGURATION  INTO  A  DIRECTED 
LINE  SEGMENT  USING  Rmtn-CIRCULAR-ARCS 


This  section  attempts  to  solve  only  the  basic  problem  of  merging  p  into  e  using 
only  circular  arcs  and  tangent  segments.  That  is,  p  is  subject  to  Kmax  constraint 
only,  and  reversals  are  allowed.  A  predefined  cap  of  k  is  placed  on  the  maximum 
number  of  maneuvers  allowed  in  a  solution  path. 

In  [Ref.  16],  P.  Jacobs  and  J.  Canny  show  that  it  is  enough  to  consider  paths 
which  consist  of  circular  arc  segments  of  maximum  curvature  (the  i?mm-arcs)  and 
common  tangents  in  path  planning  subject  to  Kmaz  constraint.  The  principle  will  be 
adopted  in  this  section. 

1.  Without  Considering  Obstacles 

Consider  connecting  two  configurations  5  and  G  in  free  space  using  only 
Rjnin-ATes  and  common  tangents,  there  are  at  most  four  possible  solution  paths.  See 
Figure  6.5. 
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In  contrast,  there  are  infinitely  many  solution  paths  for  merging  a  config¬ 
uration  p  into  a  directed  line  segment  e.  If  the  L  and  R  circles  of  p  are  considered 
separately,  this  problem  can  be  divided  into  the  following  cases  according  to  the 
relative  configurations  of  a  directed  circle  against  e.  The  solution  ranges  are  shown 
in  the  figures,  where  the  distance  from  the  center  of  a  circle  to  e  is  denoted  as  z. 

Cases  of  Solution  Ranges: 

1.  Mode  CL/TtR  and  z  >  Rmin 


2.  Mode  CR/71L  and  z  >  3 Rmin 
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2.  Limiting  the  Solution  Ranges 

The  solution  ranges  presented  in  the  last  subsection  can  be  further  limited 
based  on  the  following  empirical  arguments  (mainly  targeted  for  the  end  portions 
planning  in  a  spine  net  to  be  discussed  shortly): 

•  Only  a  finite  number  of  solution  paths  can  be  considered  for  purposes  such  as 
collision  checking  or  entry  point  selections.  Therefore,  a  finite  subset  of  the 
infinite  solution  range  with  interesting  properties  should  be  sufficient. 

•  Solution  paths  which  cross  over  to  the  other  side  of  e  are  not  interesting  if 
there  are  plenty  opportunities  in  choosing  solution  paths  on  the  same  side. 

•  Only  “forward  mergings”  are  of  interest,  where  the  entry  point  of  merging  is 
in  front  of  the  projection  of  the  L  or  R  circles  on  e.  Though  a  restriction  to 
directed  line  segments,  it  is  generally  adequate  for  p  to  merge  into  undirected 
line  segments  at  any  entry  point.  See  Figure  6.6. 
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Based  on  these  empirical  arguments,  the  solution  ranges  of  the  last  sub¬ 
section  are  limited  as  follows.  Note  that  cases  2  and  3  of  the  last  subsection  are 
collapsed  into  a  single  case. 

Cases  of  Limited  Solution  Ranges: 

1.  Mode  CL/R.R  and  z  > 


2.  Mode  CR/R.L  and  z  >  Rmi„ 


3.  Mode  OL/OR  and  R^^  >  z 
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3.  Considering  Obstacles 


Existence  of  obstacles  poses  the  following  problems: 

•  The  L  and  R  circles  of  p  may  be  obstructed  by  some  obstacles  in  C-space, 
thus  reducing  to  L  and  R  arcs. 

•  The  directed  line  segment  e  may  be  partially  visible  or  be  completely  blocked 
by  some  obstacles  from  the  L  and  R  circles  or  arcs  of  p. 

Fortunately,  it  is  easy  to  see  that  so  long  as  the  limited  solution  range  is  non-empty 
for  an  interval  of  e  visible  to  a  tangential  circle  or  maneuvering  arc  of  p,  all  solution 
paths  in  the  limited  solution  range  are  guaranteed  collision-free.  See  Figure  6.7. 


Figure  6.7:  The  Collision-Free  Limited  Solution  Range 


Proposition  6.1  The  limited  solution  range  is  always  a  subset  of  the  visible  range 
of  a  configuration  p  against  a  directed  line  segment  e.  At  the  least,  there  is  always 
a  difference  incurred  at  the  front  end  of  the  visible  interval  of  e  where  the  furthest 
entry  point  of  p  (with  the  smallest  merging  deflection  angle  a)  falls  short  of  sa  as 
shown  in  Figure  6.7. 

Proposition  6.2  All  solution  paths  in  the  limited  solution  range  are  guaranteed 
collision-free. 
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With  these  considerations  in  mind,  the  following  empirical  procedure  can 
be  employed  for  merging  a  configuration  p  into  a  directed  line  segment  e  amidst 
obstacles.  Note  that  the  procedure  is  obviously  very  much  incomplete  for  merging 
a  given  p  into  a  single  directed  line  segment  e.  But  in  later  sections,  it  can  be  seen 
that  the  procedure  is  quite  sufficient  for  merging  p  into  any  of  the  spines  in  a  spine 
net. 

Procedure  Merge: 

1.  Step  1  Construct  the  L  and  R  circles  of  p,  and  check  them  against  obstacles. 
If  none  of  them  is  collision  free,  goto  step  5. 

2.  Step  2  If  e  is  not  visible  to  either  of  the  collision-free  circles  of  p,  report  failure 
and  return. 

3.  Step  3  For  each  of  the  L  and  R  circles  of  p  that  sees  e,  do  the  following: 
If  more  than  one  intervals  of  e  are  visible  from  the  circle  because  of  multiple 
obstacles  standing  in  between  p  and  e,  choose  the  interval  that  “contains”  or 
“is  the  nearest  in  front  of”  the  projection  of  the  circle  on  e.  Make  this  interval 
the  new  e  (see  Figure  6.8).  Otherwise,  there  is  only  one  interval  of  e  visible  to 
the  circle;  make  this  interval  the  new  e.  Determine  the  limited  solution  range 
of  the  circle  against  e  (can  be  empty). 

4.  Step  4  If  both  the  limited  solution  ranges  of  the  L  and  R  circles  of  p  are 
empty,  report  failure  and  return.  Else,  goto  step  10. 

5.  Step  5  Break  the  L- arc  of  p  nto  forward  L-arc  and  backward  L- arc.  Do  the 
same  thing  to  the  R- arc.  There  are  now  four  maneuvering  arcs  of  p  under 
consideration.  Let  i  denote  the  number  of  maneuvers  so  far.  Set  i  =  0. 

6.  Step  6  For  each  of  the  forward  L  and  R  arcs  and  the  backward  L  and  R  arcs, 
do  the  following: 
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(a)  Step  6.1  If  e  is  not  visible,  set  the  limited  solution  range  of  the  arc  to 
empty,  and  continue  with  the  next  iteration  loop  of  step  6. 

(b)  Step  6.2  If  more  than  one  intervals  of  e  are  visible  from  the  arc  because 
of  multiple  obstacles  standing  in  between  p  and  e,  choose  the  interval  that 
“contains”  or  “is  the  nearest  in  front  of”  the  projection  of  the  arc  on  e. 
Make  this  interval  the  new  e.  Otherwise,  there  is  only  one  interval  of  e 
visible  to  the  arc;  make  this  interval  the  new  e.  Determine  the  limited 
solution  range  of  the  arc  against  e  (can  be  empty). 

7.  Step  7  If  any  of  the  limited  solution  ranges  of  the  four  arcs  are  non-empty, 
goto  step  10. 

8.  Step  8  Set  i  =  i  +  1.  If  i  >  k,  where  k  is  the  maximum  number  of  maneuvers 
allowed  (a  predetermined  parameter),  then  report  failure  and  return. 

9.  Step  9  From  the  extremities  of  each  of  the  four  arcs,  generate  four  new 
collision-free  maneuvering  arcs  which  are  opposite  in  both  the  modes  and  di¬ 
rections.  Goto  step  6. 

10.  Step  10  At  this  step,  a  number  of  candidate  limited  solution  ranges  from  each 
of  the  collision-free  circles  or  arcs  of  p  against  visible  e  have  been  obtained. 
For  each  of  the  limited  solution  ranges,  choose  a  candidate  solution  path  with 
the  largest  merging  deflection  angle,  that  is,  closest  to  the  projection  point  of 
the  circles  or  arcs  of  p  on  e.  From  Proposition  VI. 2  and  VI.  1,  these  candidate 
solution  paths  are  guaranteed  to  be  collision-free.  Pick  one  of  them  with  the 
least  number  of  backward  arcs  as  the  solution  path.  If  there  is  a  tie,  pick  the 
one  with  the  least  number  of  arcs.  If  there  is  a  tie  again,  pick  one  randomly. 
Report  success  and  return  the  solution  path. 
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Figure  6.8:  Partially  Visible  e  and  the  New  Truncated  e 
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D.  DIVERGING  A  CONFIGURATION  OUT  OF  A  DI¬ 
RECTED  LINE  SEGMENT  USING  Rmin-CIRCULAR- 
ARCS 

Because  of  the  symmetry  of  the  solution  path  between  the  merging  problem  and 
the  diverging  problem,  it  is  not  necessary  to  design  another  procedure  for  solving 
the  diverging  problem.  Rather,  we  can  invert  the  diverging  problem  to  the  merging 
problem  of  the  last  section,  solve  it,  then  invert  the  solution  back  for  the  diverging 
problem. 

More  specifically,  the  procedure  for  solving  the  diverging  problem  is  as  follows: 
Procedure  Diverge: 

1.  Step  1  Invert  the  orientation  of  p  and  e. 

2.  Step  2  Solve  the  problem  of  merging  the  inverted  p  into  the  inverted  e  using 
Procedure  Merge  of  the  last  section. 

3.  Step  3  If  no  inverted  merging  solution  can  be  found,  report  failure  and  return. 
Else,  invert  the  order  and  orientation  of  all  path  segments  (and  maneuvering 
directions)  of  the  inverted  merging  path  to  obtain  the  desired  solution  path  for 
diverging.  Report  success  and  return. 
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E.  MANEUVERING  BETWEEN  A  CONFIGURATION 
AND  A  DIRECTED  LINE  SEGMENT  USING  CUBIC 
SPIRALS 

Recall  that  one  of  the  major  consideration  of  using  cubic  spirals  instead  of 
circular  arcs  is  to  allow  non-stop  forward-moving  motions  for  p  in  making  smooth 
turns  during  the  middle  portion  of  a  motion  path.  However,  at  the  end  portions 
(the  initial  and  the  final  portions),  stops  and  reversals  are  allowed,  and  only  feasible 
paths  are  called  for.  Therefore,  in  end  portions  path  planning: 

•  It  is  not  necessary  to  replace  circular  arcs  by  cubic  spirals,  because  p  will  have 
to  halt  at  the  extremities  anyway.  Circular  arcs  will  suffice. 

•  The  only  exceptions  are:  cubic  spiral  is  used  for  the  last  maneuver  of  p  merging 
into  e,  and  for  the  first  maneuver  of  p  diverging  out  of  e.  In  fact,  these  two 
places  defines  the  intermediate  So  and  Go  configurations  called  for  in  the  prob¬ 
lem  statement  of  this  thesis.  They  are  also  the  demarcation  points  between 
the  middle  portion  and  the  end  portions  of  a  solution  path. 

In  the  discussions  that  follows,  only  Kmax  constraint  is  considered  at  first  for 
merging  p  into  e  in  the  initial  phase  using  cubic  spirals.  Then  an  additional  kmax 
constraint  is  imposed  on  the  cubic  spiral.  Lastly,  the  problem  of  diverging  p  out  of  e 
in  the  final  phase  using  cubic  spirals  subject  to  both  constraints  is  considered.  This 
section  makes  heavy  references  to  the  last  two  sections,  because  paths  consisting 
of  circular  arcs  are  the  stepping  stones  towards  constructing  those  of  cubic  spirals. 
Obstacles  are  considered  throughout  this  section. 
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1.  Merging  p  into  e  Using  Cubic  Spirals  Subject  to  nmax 
Constraint  Only 

Procedure  Merge  as  described  in  subsection  3.  can  be  modified  as  follows 
to  find  a  solution  path  using  cubic  spirals  for  the  last  maneuver  of  p  into  e  in  the 
initial  portion  planning: 

Modifications  to  Procedure  Merge: 

1.  Instead  of  using  iLnm-circles  in  Procedure  Merge ,  use  rmi„-circles,  where 

3 

rmin  —  0 

+>Kmax 

This  is  to  ensure  full  /cmox-admissibility  for  the  circumscribing  cubic  spirals.1 
All  steps  in  Procedure  Merge  are  affected  by  this  modification. 

2.  For  the  last  maneuver  in  the  initial  portion  planning,  if  a  circular  turn  is 
required  on  the  maneuvering  arc  of  p  in  the  forward  direction,  it  must  be 
replaceable  by  a  collision-free  circumscribing  cubic  spiral  (see  Figure  6.9).  Add 
an  additional  step  6.3  to  Procedure  Merge  as  follows: 

•  Step  6.3  Let  q  denote  the  configuration  on  the  last  maneuvering  arc  of 
p  with  the  largest  merging  deflection  angle  in  the  limited  solution  range. 
If  p  =  q,  or  the  last  maneuvering  arc  is  a  backward  arc,  then  make  p  a 
candidate  So  called  for  in  the  problem  statement  and  continue  with  the 
next  iteration  loop  of  step  6.  Else,  construct  the  bounding  region  ( BR\ 
or  BR2 )  of  the  cubic  spiral  circumscribing  on  p  and  q.  If  this  bounding 
region  is  collision-free,  make  p  a  candidate  S0.  Otherwise,  make  q  the 
candidate  So- 

'Although  the  /Z^n-circle  can  still  be  used  for  the  circular  maneuvering  arcs,  such  minor  opti¬ 
mization  is  omitted  for  the  sake  of  simplicity. 
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3.  For  the  last  maneuver  in  the  initial  portion  planning,  the  circular  arc  merging 
into  e  has  to  be  replaced  by  a  cubic  spiral.  It  can  be  easily  seen  that  this 
circular  arc  and  its  two  tangents  (one  of  which  lies  on  e)  form  a  bounding 
region  BR\  of  the  cubic  spiral  circumscribing  the  circular  arc.  Since  BR\  is 
contained  in  the  visible  region  between  the  maneuvering  arc  of  p  and  e,  BR\ 
(and  hence  the  cubic  spiral)  are  clearly  collision-  free  (see  Figure  6.9).  In  step 
10  of  Procedure  Merge ,  simply  replaces  all  circular  arcs  in  the  last  maneuvers 
by  their  circumscribing  cubic  spirals. 
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2.  Merging  p  into  e  Using  Cubic  Spirals  Subject  to  an 
Additional  kmax  Constraint 

Assuming  that  (3  is  the  partially  Km0I-admissible  angle  for  the  rmi„-circle. 
Procedure  Merge  has  to  be  further  modified  as  follows  to  cater  for  an  additional  kmax 
constraint: 

Further  Modifications  to  Procedure  Merge: 

1.  The  furthest  entry  point  of  p  into  e  is  restricted  to  have  merging  deflection 
angle  a  >  (see  Figure  6.10).  Although  this  constraint  sometimes  further 
reduces  the  limited  solution  range,  it  guarantees  that  the  problem  of  partial 
«ma*-admissibility  can  be  avoided  all  together  for  the  circular  arc  merging  into 
e  during  the  final  maneuver.  Step  3  and  step  6.2  of  Procedure  Merge ,  which 
are  responsible  for  constructing  the  limited  solution  range,  are  affected  by  this 
modification. 

2.  Modify  step  6.3  and  add  additional  steps  6-4  and  6.5  to  Procedure  Merge  as 
follows: 

•  Step  6.3  Let  q  denote  the  configuration  on  the  last  maneuvering  arc  of 
p  with  the  largest  merging  deflection  angle  in  the  limited  solution  range. 
If  P  =  9,  or  the  last  maneuvering  arc  is  a  backward  arc,  then  make  p  a 
candidate  50  called  for  in  the  problem  statement  and  continue  with  the 
next  iteration  loop  of  step  6.  Else,  if  the  deflection  angle  from  p  to  q  is 
>  /?,  goto  step  6.5. 

•  Step  6.4  Let  q'  denote  the  configuration  on  the  final  maneuvering  arc  of 
p  with  the  smallest  merging  deflection  angle  (>  /?)  in  the  limited  solution 
range.  If  the  deflection  angle  from  p  to  q'  is  >  /?,  make  q'  to  be  q ,  and 
goto  step  6.5.  Else,  reset  the  limited  solution  range  of  this  maneuvering 


101 


arc  against  c  to  empty,  and  continue  with  the  next  iteration  loop  of  step 

6. 

•  Step  6.5  Construct  the  bounding  region  ( BR\  or  BR2)  for  the  cubic 
spiral  circumscribing  on  p  and  q.  If  this  bounding  region  is  collision-free, 
make  p  a  candidate  Sq.  Otherwise,  make  q  the  candidate  So. 


Figure  6.10:  The  Collision-Free  Limited  Solution  Range  subject  to  Kmor  Constraint 

3.  Diverging  p  out  of  e  Using  Cubic  Spirals  Subject  to 
Both  Kmax  and  kmax  Constraints 

As  is  the  case  of  the  circular  arc,  the  cubic  spiral  is  also  symmetric.  There¬ 
fore,  Procedure  Diverge  as  described  in  section  D.  can  be  followed  exactly  to  solve 
the  problem  of  diverging  p  out  of  e  using  cubic  spirals  subject  to  both  the  «mor  and 
Kmji  Constraints  for  the  final  portion  planning.  Of  course,  the  modified  Procedure 
Merge  is  employed  in  finding  the  inverted  solution  path  in  Procedure  Diverge. 


102 


F.  END  PORTIONS  PATH  PLANNING  FOR  THE  SPINE 
NET  USING  CUBIC  SPIRALS 

This  section  is  a  culmination  of  the  techniques  developed  in  the  previous  sections 
for  end  portions  path  planning  for  the  spine  net.  The  following  procedure  simply 
applies  Procedure  Merge  and  Procedure  Diverge  to  5  and  G  respectively  against  all 
directed  spines  in  the  spine  net,  thereby  generating  many  candidates  of  So  and  Go- 

Procedure  End  Portions  Path  Planning 

1.  Step  1  For  each  of  the  directed  spines,  do  the  following: 

Use  Procedure  Merge  to  check  if  it  is  possible  to  merge  S  into  it.  If  yes,  note 
down  the  candidate  So  and  the  entry  point  of  this  directed  spine. 

2.  Step  2  If  the  set  of  candidate  So  is  empty,  report  failure  and  return. 

3.  Step  3  For  each  of  the  directed  spines,  do  the  following: 

Use  Procedure  Diverge  to  check  if  it  is  possible  to  diverge  G  out  of  it.  If  yes, 
note  down  the  candidate  Go  and  the  exit  point  of  this  directed  spine. 

4.  Step  4  If  the  set  of  candidate  Go  is  empty,  report  failure  and  return. 

There  is  a  fine  point  that  can  be  made  about  Procedure  End  Portions  Path 
Planning.  At  the  moment,  the  last  maneuver  into  a  directed  spine  during  the  initial 
portion  planning,  and  the  first  maneuver  out  of  a  directed  spine  during  the  final 
portion  planning,  are  always  forward-moving.  In  case  there  is  a  narrow  passage 
where  the  only  possible  last  maneuver  for  5  is  to  reverse  out,  or  the  only  possible 
first  maneuver  at  G  is  to  reverse  in,  Procedure  End  Portions  Path  Planning  will 
always  fail.  It  is  surprisingly  easy  to  handle  these  cases  by  adding  some  more  steps 
into  Procedure  End  Portions  Path  Planning  in  conjunction  with  the  Procedure  Graph 
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Search  to  be  discussed  in  the  next  chapter.  The  necessary  changes  are  sketched  as 
follows  without  increasing  the  time  complexity  of  the  operations: 

1.  In  case  Procedure  End  Portions  Path  Planning  fails  to  find  a  candidate  So  or 
G0,  reverse  the  direction  of  S  and/or  G  accordingly,  and  apply  the  procedure 
again. 

2.  If  a  candidate  can  be  found  this  time,  apply  Procedure  Graph  Search  to  obtain 
an  auxiliary  candidate  solution  path.  Else,  report  failure. 

3.  If  it  is  S  that  is  in  question,  find  the  first  junction  on  the  auxiliary  candidate 
solution  path  where  it  is  possible  for  p  to  turn  into  an  opposite  direction.  Then 
it  is  obvious  that  p  could  reverse  all  the  way  from  S  to  this  point,  and  resume 
forward-moving  motion  from  this  point  on.  Note  that  because  of  symmetry, 
it  is  possible  to  obtain  a  forward-moving  path  by  reversing  the  directions  of  a 
backward-moving  path. 

4.  Similar  argument  applies  to  the  case  of  G. 

This  refinement  is  not  included  in  Procedure  End  Portions  Path  Planning  for 
a  simple  reason:  this  procedure  and  the  overall  layered  motion  planning  problem 
using  spine  net  is  incomplete.  They  are  practical  because  office-like  environment  is 
assumed  where  there  are  ample  leeway  for  maneuvering.  Consideration  of  narrow 
passage  which  requires  p  to  reverse  out  of  5  or  into  G  is  just  not  consistent  with 
the  assumption,  and  is  only  of  theoretical  interest  (although  this  refinement  offers 
to  solve  the  narrow-passage  problem  gracefully). 
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G.  TIME  COMPLEXITY  ANALYSIS 


Referring  to  the  data  structure  of  the  spine  net  discussed  in  Section  4.,  there 
are  0(m)  directed  spines  in  a  spine  net.  In  the  case  of  sensor-oriented  spine  net, 
m  —  0(n),  where  n  is  the  number  of  obstacle  walls  in  W.  In  the  case  of  freeway 
spine  net,  m  =  0(n2). 

Both  Procedure  Merge  and  Procedure  Diverge  take  0(nk)  time.  Hence  Procedure 
End  Portions  Path  Planning  takes  0(mnk)  time. 

Conclusion  Procedure  End  Portions  Path  Planning  takes  O(mnk)  time. 
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VII.  GRAPH  SEARCH  LAYER 


A.  OVERVIEW 

The  graph  search  layer  is  the  fourth  and  the  last  layer  in  the  layered  motion 
planning  approach  proposed  in  this  thesis.  In  this  layer,  a  Dijkstra’s  search  is  made 
for  all  candidate  So  and  Go  (generated  by  the  end  portions  path  planning  layer)  on 
the  smooth  At,  producing  a  number  of  candidate  solution  paths.  Subsequently,  an 
optimal  solution  path  from  S  to  So  to  G0  to  G  is  selected  out  of  these  candidate 
solution  paths  based  on  a  predefined  cost  function.  Note  that  this  solution  path 
is  the  most  optimal  only  in  the  context  of  the  layered  motion  planning  approach 
proposed  in  this  thesis.  This  solution  path  is  also  a  qualified  solution  to  the  problem 
statement  set  forth  in  this  thesis. 

Notes: 

1.  Factors  that  can  be  considered  in  a  cost  function  include: 

•  path  length 

•  path  smoothness 

•  safety 

•  number  of  turns 

•  number  of  reversals 

•  wall  following  distance 

Actual  definition  of  a  cost  function  is  not  important  in  theoretical  studies. 
However,  in  actual  applications,  specific  cost  functions  must  be  defined  for 
each  specific  case. 
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2.  Although  the  graph  search  procedure  developed  in  this  chapter  is  worded 
specifically  for  the  spine  net,  the  general  principle  is  applicable  to  other  classes 
of  roadmaps  as  well. 

B.  PROCEDURE  GRAPH  SEARCH 

Given  a  number  of  candidate  So  and  Go  generated  by  Procedure  End  Portions 
Path  Planning  of  the  last  layer,  the  following  procedure  produce  an  optimal  solution 
path  from  S  to  So  to  Go  to  G  based  on  a  predefined  cost  function. 

Procedure  Graph  Search 

1.  Step  1  For  each  of  the  candidate  Go,  do  the  following: 

(a)  Step  1.1  Associated  with  each  candidate  Go  are  a  directed  spine  es  that 
it  diverges  out  of,  and  the  corresponding  exit  point  Gj  on  eg.  Make  G\  an 
auxiliary  vertex  in  M  with  no  outgoing  edges. 

(b)  Step  1.2  Compute  the  path  cost  from  Gx  to  G0  to  G  according  to  the 
predefined  cost  function.  Assign  this  cost  as  the  initial  cost  of  the  auxiliary 
vertex  G\. 

2.  Step  2  For  each  of  the  candidate  So,  do  the  following: 

(a)  Step  2.1  Associated  with  each  candidate  So  are  a  directed  spine  e,  that 
it  merges  into,  and  the  corresponding  entry  point  S\  on  e,.  Make  Sj  an 
auxiliary  vertex  in  M.  with  no  incoming  edges. 

(b)  Step  2.2  Compute  the  path  cost  from  S  to  So  to  Si  according  to  the 
predefined  cost  function.  Assign  this  cost  as  the  initial  cost  of  the  auxiliary 
vertex  Sj. 

(c)  Step  2.3  Make  a  Dijkstra’s  Search  of  M  starting  from  the  auxiliary  vertex 
Si.  Whenever  an  auxiliary  vertex  Gi  is  reached,  a  candidate  solution  path 
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from  S  to  Sq  to  S\  to  G\  to  Gq  to  G  is  found.  This  candidate  solution 
path  is  also  the  least  expensive  one  that  passes  through  the  candidate  Sq 
in  this  step  2  loop.  Terminate  the  Dijkstra’s  Search  and  continue  with 
the  next  loop  iteration.  Note  that  the  Dijkstra’s  Search  must  be  modified 
to  include  the  initial  costs  of  S\  and  G\. 

3.  Step  3  If  none  of  the  auxiliary  vertices  G\  is  reached  in  step  2 ,  report  failure 
and  return. 

4.  Step  4  Of  all  the  auxiliary  vertices  G\  reached  in  step  2 ,  pick  the  candidate 
solution  path  with  the  least  cost.  This  is  the  most  optimal  solution  path  found 
using  the  layered  motion  planning  approach  proposed  in  this  thesis.  It  is  also 
a  solution  to  the  problem  statement  of  this  thesis. 

C.  TIME  COMPLEXITY  ANALYSIS 

In  the  case  of  a  spine  net  M,  there  are  potentially  0(01^)  vertices  and  0{m4) 
edges,  where  m  is  the  number  of  spines  in  the  spine  net.  Hence  a  Dijkstra’s  Search 
on  M  can  be  carried  out  in  0{m4)  time. 

The  end  portions  path  planning  layer  can  generate  up  to  0(m)  number  of 
candidate  So  and  Go-  Hence  Procedure  Graph  Search  takes  up  a  total  of  0(m5) 
time. 

In  the  case  of  sensor-oriented  spine  net,  m  =  0(n),  where  n  is  the  number  of 
obstacle  walls  in  W .  In  the  case  of  freeway  spine  net,  m  =  0(n2). 

Conclusion  Procedure  Graph  Search  takes  0(m5)  time. 
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VIII.  IMPLEMENTATION  PLAN 


A.  INTRODUCTION  TO  YAMABICO-11  ROBOT 

Yamabico-ll  is  a  research  robot  under  continuous  development  by  students  and 
faculty  at  the  Naval  Postgraduate  School.  It  is  a  Power  Wheeled  Steering  (PWS) 
type  vehicle,  and  can  be  closely  approximated  by  a  disc-shaped  rigid  body  robot 
in  its  planar  applications.  Its  low  level  motion  control  functions  allow  it  to  follow 
circular  paths,  cubic  spiral  paths  and  straight  paths  (which  are  just  special  cases  of 
cubic  spiral  paths).  At  present,  seven  sonar  sensors  mounted  on  it  are  fully  functional 
which  allows  position  and  orientation  correction  using  sonar  returns. 

The  targeted  operating  environment  of  Yamabico-ll  robot  is  an  in-door  office 
environment  with  mostly  rectilinear  type  of  constructions  and  obstacles.  Because  of 
engineering  imprecisions,  it  is  imperative  for  Yamabico-ll  robot  to  perform  contin¬ 
uous  position  and  orientation  correction  when  following  a  preplanned  motion  path. 
The  quality  of  the  sonar  return  is  best  when  the  robot  navigates  in  parallel  with  an 
obstacle  wall.  Therefore,  it  is  important  that  the  motion  planar  of  the  robot  plans 
motion  paths  that  follows  obstacle  walls  in  an  optimal  way. 

In  this  respect,  the  layered  motion  planning  approach  using  sensor-oriented 
method  in  the  global  path  planning  layer  is  directly  applicable  to  the  Yamabico-ll 
robot. 
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B.  KINEMATIC  CONSTRAINTS  OF  YAMABICO-11  ROBOT 


Although  Yamabico-11  is  a  non-holonomic  robot  (meaning  that  its  heading 
direction  (reference  orientation)  must  be  equal  to  the  tangent  orientation  of  the 
motion  path),  it  can  rotate  about  its  center  when  it  is  stationary.  This  allows  it  to 
follow  paths  consisting  of  only  straight  line  segments  with  frequent  stoppings  at  each 
junction.  Yamabico-11  is  also  not  subject  to  any  curvature  constraint  because  it  is 
a  PWS  type  vehicle. 

However,  it  would  be  desirable  to  plan  motion  paths  in  an  office-like  environment 
which  Yamabico-11  can  navigate  with  non-stopping  and  forward-moving  motion. 
Such  a  requirement  in  effect  imposes  three  artificial  constraints,  namely  the  C2  path 
class,  the  Kmax  and  kmax  constraints,  on  Yamabico-11  depending  on  its  traveling 
speed. 

1.  Let  the  tread  of  Yamabico-11  robot  be  2 W .  According  to  [Ref.  17],  if  the  robot 
is  moving  at  a  speed  u,  the  speed  Vi  and  vT  at  the  left  and  right  wheels  are: 


|  v  =  (1  —  VY/c)v 

(8-1) 

(r  + 
vT  =  ( 

\  r  > 

|  v  =  (1  +  lf/c)v 

(8.2) 

where  k  is  the  curvature  of  the  motion  path. 

These  equations  completely  describes  the  kinematics  of  PWS  vehicles.  They 
show  that  the  speeds  of  both  the  left  and  right  wheels  are  linear  functions 
of  «.  Therefore,  if  /c(s)  is  discontinuous,  the  vehicle  is  not  able  to  follow  the 
path  unless  it  stops  at  those  points.  If  Yamabico-11  robot  is  to  navigate  in  a 
non-stop  motion,  its  motion  path  must  have  curvature  continuity,  that  is  its 
motion  path  must  be  of  class  C2 .  An  example  is  a  path  consisting  of  cubic 
spiral  segments. 
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2.  Let  to  be  the  mass  of  Yamabico-11  robot.  Assuming  constant  traveling  speed 


t>  for  Yamabico-11,  the  centripetal  force  during  a  turn  is: 

.2 


mv  o 

-  =  mv  k 


(8.3) 


If  Fmax  is  the  maximum  friction  force  between  the  wheels  of  Yamabico-11  and 
the  floor,  to  avoid  slippage,  we  must  have: 


Fmax  >mVK 


or 


mv * 


(8.4) 


3.  Let  amax  be  the  maximum  acceleration  attainable  by  Yamabico-11.  Assuming 
constant  traveling  speed  v.  From  Equation  8.1  and  8.2,  we  have: 


*raai  ^ 


dvrj 

dt 

^([1  ±  Wk)v) 


...  dK 

— 

±Wv— 

dt 

=  Wv 


( 


die' 

ds 


'd£ 

dt 


=  Wv2k 


or 


ftmax 


Wv 2 


(8.5) 


These  artificial  constraints  imposed  on  Yamabico-11  are  considered  in  the  prob¬ 
lem  statement  of  this  thesis.  Which  means  that  the  result  of  this  thesis  can  be  readily 
tailored  for  implementation  on  Yamabico-11. 
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C.  DESIRABLE  MOTION  PATH  CHARACTERISTICS 


Given  S  and  G ,  the  problem  statement  of  this  thesis  calls  for  intermediate  So 
and  Go,  such  that  only  the  middle  portion  of  the  path  from  S0  and  G0  need  to  be 
smooth.  The  initial  portion  of  the  path  from  S  to  So,  and  the  final  portion  from  Go 
to  G  can  be  just  feasible  which  allows  for  reversals. 

In  the  case  of  Yamabico-11,  it  can  rotate  about  its  center  when  it  is  stationary 
at  point(S)  and  point(G).  This  in  effect  eliminates  the  requirement  of  So  and  Go- 
It  is  thus  possible  to  directly  plan  a  smooth  path  from  point(S)  to  point(G). 

However,  it  may  be  academically  more  interesting  to  impose  the  kinematic 
constraints  on  Yamabico-11  robot  even  when  it  is  stationary.  It  would  also  allow 
the  result  of  this  thesis  to  be  directly  implementable  on  Yamabico-11  without  any 
tailoring  effort. 

D.  SPECIFIC  COST  FUNCTION 

Recall  that  in  the  graph  search  layer,  a  specific  cost  function  is  required  to  be 
defined  for  each  specific  application. 

The  following  cost  function  is  defined  for  a  path  obtained  from  the  sensor- 
oriented  method  for  Yamabico-11: 

fG 

cost  =  J  c(p)  ds  (8.6) 

where  p  is  a  point  on  the  path,  and, 

.  .  _  f  c i  if  wmin  <  w(p)  <  wmax  A  6(p)  is  parallel  to  an  obstacle  wall  A  k (p)  =  0 
(  c2  otherwise,  where  c2  >  cx 

In  plain  language,  path  segments  that  are  parallel  to  obstacles  walls  and  are 
within  the  sonar  range  are  of  lower  cost.  All  other  path  segments,  including  cubic 
spiral  turns,  circular  maneuver  arcs,  spine  extensions  and  the  straight  path  segments 
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involved  in  the  end  portions,  are  of  higher  cost  because  they  do  not  facilitate  position 
correction  using  sonar  returns. 

E.  ADDITIONAL  CONSIDERATIONS  FOR  IMPLEMEN¬ 
TATION 

1.  The  disc-shaped  robot  T)  approximating  Yamabico-11  should  be  grown  by  a 
safety  bumper  of  A r  to  accommodate  dead  reckoning  errors.  Even  if  the  po¬ 
sition  and  orientation  correction  capability  is  activated,  the  corrections  will 
be  carried  out  only  periodically  and  dead  reckoning  errors  will  still  occur  in 
between  two  corrections. 

2.  The  initial  and  final  configurations  are  given  explicitly  per  path  planning  query. 
Yamabico-11  cannot  compute  its  initial  configuration  autonomously  at  the  mo¬ 
ment  because  the  vision  matching  technique  is  still  premature  for  implemen¬ 
tation.  Some  other  higher  level  knowledge  representation  of  W  may  also  be 
required  for  Yamabico-11  to  recognize  its  whereabouts  in  W  besides  brute-force 
matching  of  the  geometry  of  W.  The  manual  input  of  the  initial  configuration 
can  be  expected  to  suffer  higher  level  of  error  in  accuracy  than  that  of  the  dead 
reckoning  capability  of  Yamabico-11.  Therefore,  the  safety  bumper  must  be 
padded  up  by  another  measurement-error  bumper  of  Ae. 
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IX.  CONCLUSION 


Motivated  by  solving  a  specific  motion  planning  problem  for  Yamabico-11,  the 
problem  is  generalized  into  an  academic  research  topic  of  this  thesis  of  which  the 
Yamabico-11  application  is  but  a  special  case. 

The  results  of  this  thesis  can  be  applied  to  motion  planning  problems  for  any 
non-holonomic  disc-shaped  robots  which  are  subject  to  kinematic  constraints  of  nmax 
and  kmaI.  Although  the  results  are  inexact,  they  should  be  sufficient  in  applications 
where  the  target  operating  environment  is  an  office-like  environment.  The  results 
should  also  allow  fast  computation  that  will  facilitate  real-time  applications.  The 
motion  paths  planned  will  have  a  nice  property  that  except  at  the  end  portions 
where  reversals  are  usually  inevitable,  the  majority  of  the  middle  portion  will  allow 
non-stopping  and  forward-moving  motions.  To  the  best  of  the  author’s  knowledge, 
no  other  results  have  produced  paths  of  these  properties  subject  to  Kmax  and  kmax 
constraints. 

Research  contributions  of  this  thesis  include: 

1.  Formalization  of  the  layered  motion  planning  approach  framework 

Although  breaking  down  the  motion  planning  problem  into  two  or  three  layers 
has  been  informally  considered  by  researchers,  this  thesis  is  the  first  in  formally 
laying  down  the  framework  of  four-layered  motion  planning  approach.  The 
goal  of  each  layer,  together  with  the  interfaces  between  two  adjacent  layers, 
are  defined  explicitly.  Different  methods  are  applied  using  the  layered  approach 
which  demonstrates  the  versatility  of  such  a  framework. 
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2.  The  novel  sensor-oriented  global  path  planning  method 

Of  all  the  available  global  path  planning  method,  none  builds  a  roadmap 
that  facilitates  sensor-oriented  robots  in  carrying  out  position  and  orientation 
correction  using  sonar  returns.  This  thesis  proposes  a  novel  sensor-oriented 
method  which  builds  a  roadmap  with  spines  running  parallel  to  obstacles  as 
far  as  possible  and  within  the  sonar  range.  Yet  it  avoids  becoming  a  wall¬ 
following  algorithm  by  letting  a  cost  function  chooses  shortcuts  wherever  more 
economically. 

3.  Investigation  of  smooth  path  of  class  C 2  subject  to  Kmor  and  kmax  constraints 

This  thesis  investigates  the  problem  of  planning  smooth  path  of  class  C 2  subject 
to  KmiI  and  kmax  constraints.  Such  paths  are  very  appealing  because  they  can 
be  followed  by  many  different  types  of  rigid  body  robot  without  stopping. 
However,  in  most  research  papers,  only  C 1  paths  of  piecewise  C2  subject  to 
fcmax  constraint  are  investigated.  The  kmax  constraint  has  not  been  investigated 
to  the  best  of  the  author’s  knowledge.  Specific  techniques  of  transforming  a 
global  roadmap  into  a  smooth  roadmap  of  class  C2  are  discussed  in  the  local 
smooth  path  planning  layer. 

4.  Detailed  study  of  the  properties  of  cubic  spirals 

A  class  of  smooth  curves  called  cubic  spirals  is  adopted  in  this  thesis  for  plan¬ 
ning  smooth  C2  paths.  Previous  studies  on  the  properties  of  cubic  spirals 
for  path  planning  are  made  in  isolated  context,  where  obstacle  collisions  and 
kinematic  constraints  are  not  considered.  Because  of  this,  the  elegant  curve 
of  cubic  spiral  could  not  find  its  way  to  applications  where  the  full  context  of 
path  planning  are  to  be  considered.  This  thesis  investigates  further  properties 
of  cubic  spirals  subject  to  Kmax  and  kmax  constraints.  It  fills  in  the  missing 
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links  for  the  cubic  spiral  by  proposing  using  bounding  regions  BRX  and  BR 2  to 
approximate  the  trajectory  of  a  cubic  spiral  which  has  no  closed  form.  For  the 
first  time  a  practical  approximate  algebraic  solution  is  available  for  collision 
checking  against  the  cubic  spiral. 

5.  Investigation  of  end  portions  path  planning 

In  many  research  papers,  the  end  portions  path  planning  problem  is  either 
avoided  (by  assuming  S  and  G  lie  on  the  roadmap)  or  briefly  mentioned  without 
outlining  the  planning  procedures.  This  problem  is  studied  in  detail  in  the  end 
portions  path  planning  layer,  and  a  planning  procedure  is  specified  explicitly. 
Although  there  are  still  rooms  for  improvement,  such  effort  is  the  first  attempt 
in  tackling  the  end  portions  path  planning  problem  head  on. 

One  important  area  that  is  open  for  future  research  is  a  good  definition  for  path 
cost  function.  As  pointed  out  in  Chapter  VII,  there  are  many  factors  that  can  be 
included  in  a  path  cost  function.  In  this  thesis,  only  a  composite  cost  function  of 
wall-following  distance  versus  shortness  is  proposed.  Other  important  factors  such 
as  number  of  reversals,  number  of  turns,  etc,  are  ignored.  It  is  still  an  open  question 
on  how  to  blend  all  these  factors  into  a  single  uniform  cost  function. 

Another  interesting  topic  for  future  research  is  to  extend  the  results  of  this 
thesis  to  polygon-shaped  robots,  for  example,  car-like  vehicles. 
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